0 引　言

近年来，各国均加快了无人潜航器研发的进度。UUV装备应用到海洋环境考察^[1]、反潜^[2]、反水雷^[3]、情报搜集^[4]等任务的案例报道也愈发增多。随着单个无人潜航器应用场景的不断丰富，多UUV协同控制的发展也渐渐出现。

UUV协同不仅可以丰富无人潜航器的作业场景，增强鲁棒性，而且在一些特殊场景下，需要多个UUV作业单元同步配合，完成任务需求如协同搜探、协同打击等任务场景。这样一来，无人潜航器系统的发展更加离不开协同控制技术，无人潜航器以集群的形式互相协作执行任务是未来水下任务发展的必然趋势^[5]。国内外针对UUV协同问题也开展了若干研究，美国海军研究院资助的自主海洋采样网^[6]（Autonomous Ocean Sampling Network，AOSN）项目，以水下滑翔机为核心平台，构建海洋观测网，观测大范围近海和沿海，并分别于2003年和2006年多次在美国蒙特利湾进行了一系列的海洋观测试验，可应用于构建海洋模型、实现环境预测，此外，欧盟从2014年开始，开展了名为“可扩展移动水声组网技术”（WiMUST）的研究项目^[7]，旨在建立一套包含多个UUV的可扩展水声组网系统，用于大范围海底地形测绘、海洋水文采样、海洋搜救、海洋考古以及敌情侦察、雷区搜索等任务，并于2016年11月在葡萄牙附近海域开展了海上试验，实现了10余套UUV的协同导航和网络化控制。可见多UUV协同能力对于分布范围广泛的海军作战保障有着非常重要的意义。其中，水下协同路径规划是协同任务执行的一个必要环节，协同路径规划算法的能力直接影响着协同效能指标的发挥程度。

水下协同路径规划技术面临着水声通信丢包率大、延迟高，且存在通信盲区问题，协同过程中，UUV之间无法实时建立通信连接，且实际水下环境复杂，可能存在若干未知障碍物。为了解决上述问题，本文针对水下协同任务场景中根据UUV群组接收到的上层任务指令，集中式分配每台UUV对应的任务航点，并采用粒子群优化与B样条曲线相结合的方式完成分布式离线轨迹生成，形成满足UUV前向速度、转弯半径等一系列运动学约束条件的可行轨迹，针对轨迹跟踪过程中遇到未知障碍物的问题，采用避碰域与人工势场法结合方式，实现UUV集群在跟踪轨迹过程中的在线动态避障路径规划。并设计虚拟点制导律实现UUV的动态轨迹跟踪，从而形成一套较为完整的UUV协同控制技术框架，并通过仿真验证协同算法的有效性，可实现UUV集群同时到达既定目标占位点的任务。

1 多UUV协同控制算法框架

为了解决多UUV协同控制中存在的水声通信条件差，难以持续维持可靠的通信服务质量以及水下环境复杂多变，存在未知障碍物条件，影响多UUV协同的问题，本文提出分层式协同控制技术方案，将任务实现过程分为群组任务决策层、群组目标规划层、群组制导规划层，具体技术方案如图1所示。

本文在分层技术方案中重点解决群组目标规划与制导规划，采用集中式分配、分布式优化控制的分层式^[8]协同控制算法框架如图2所示。

相比于集中式^[9]优化控制而言，该框架具备计算速度快、系统鲁棒性强的优点，且不同于纯分布式控制算法如拍卖算法，需要集群内部频繁交互信息，通信依赖性强，不适用于水下场景。

多UUV接收到上层输入的目标点位集合指令后，根据当前集群态势建立航路目标模型，并基于该全局信息共识执行任务分配算法，形成一对一的占位点匹配，完成集中式协同规划。之后，单UUV根据匹配的目标占位点，结合自身运动学约束、同时到达时间约束以及终点目标航向约束条件进行轨迹必达点寻优生成目标路径，再通过B-样条插值对生成的离散路径点进行平滑，完成单体的路径规划与轨迹生成，当跟踪过程中遇到障碍时，采用避碰域与人工势场结合方式实现UUV动态避碰。跟踪控制层基于结合虚拟领航点的视线角制导算法给出期望航向、航速，再由PID控制器完成底层控制，从而形成群体规划、单体规划、制导跟踪与控制一体化的协同控制框架，完成多UUV协同控制任务。

2 UUV协同任务分配

假定初始化条件下，集群内UUV个数为$N$，上层任务指令输入为$N$个带有终点航向约束的目标占位点集合。假定每台UUV在集结区域内可以获得集群内其他UUV的位置状态信息以及目标占位点的信息，UUV集合$\{ X\}$内的元素为${X_i} = \left( {x_i^u,y_i^u} \right)$，代表UUV所处的大地坐标系位置。目标占位点集合$\{ Y\}$内的元素为${Y_j} = \left( {x_j^p,y_j^p} \right)$，代表目标占位点的位置信息。

之后可以根据UUV协同任务所面临的具体场景，建立相应的任务评价模型，本文假设上层任务指令为期望UUV群组在特定运动学能力约束范围内，按最快时间同时到达指定的集结占位区域为目标，并按照约定的期望航向到达期望的目标航点位置上。

因此，本文将任务分配收益建模为式(1)所示，考虑多UUV执行协同任务时总距离最短，建立收益模型，二分图边的权值可定义如下式:

$\omega ({X_i},{Y_j}) = \frac{1}{{\sqrt {{{(x_i^u - x_j^p)}^2} + {{(y_i^u - y_j^p)}^2}} }} 。$

(1)

该问题的直观描述如图3所示。则该问题可转换为完备匹配下的最大权匹配，可以采用KM算法解决完备信息状态下的最优匹配。该方法求解效率高，便于在实际工程应用中部署到UUV控制器上。

基于KM最优匹配的任务分配算法流程如图4所示。KM算法引入了可行顶标^[10]等概念，获得最佳的完备匹配。

首先初始化可行顶标，$l_x、l_y$分别用于记录集合$\{ X\}$和集合$\{ Y\}$中的结点顶标值，设定${l_y}({y_i})$的值为0，${l_x}({x_i})$的值为与${X_i}$相关联的边最大权值$\omega ({X_i},{Y_j})$。之后，基于匈牙利算法^[11]采用增广路径求最大匹配，并对于访问过的节点修改其顶标如下式：

$d = \mathop {\min }\limits_{x \in A,y \in B} \{ l(x) + (y) - \omega (x,y)\} 。$

(2)

式中，$A \subseteq X,B \subseteq Y$。

遍历全部子图后，可得到该二分图的最优匹配，即每台UUV期望的占位点。

由此，实现UUV协同任务过程中的集中式任务分配环节，该方法可以避免UUV在协同过程中对水声通信服务质量的依赖，只需要在初始时刻UUV集群内接受设定好的全局目标信息，构成任务分配所需完备数据集，进而执行基于完备数据条件下的任务分配算法，避免UUV集群在任务分配过程中产生次优解或分配失败的问题。

3 UUV协同轨迹规划方法 3.1 轨迹规划建模

首先，对UUV协同轨迹规划问题进行建模，上层任务指令输入在一定角速度、线速度等运动学约束条件下，集群系统内的UUV按照约定的到达时间、以特定期望航向到达终点期望位置。第2节所述任务分配方法实现UUV群组任务目标的最优匹配，将UUV协同控制问题转换为UUV的群组轨迹规划问题，该问题的数学表达如下式：

$\begin{gathered} x({t_{final}}) = {x_f}{\text{ }},{\text{ }}y({t_{final}}) = {y_f}，\\ psi({t_{final}}) = ps{i_f}，\\ \omega < {\omega _c}{\text{ }},{\text{ }}u_c^{\min } < u < u_c^{\max }。\\ \end{gathered}$

(3)

其中：${\omega _c}$为UUV的角速度约束；$u_c^{\max }$为UUV速度上限；$u_c^{\min }$为UUV速度下限。

可将式(3)所述问题转换为轨迹优化目标，定义罚函数^[12]为：

$\begin{gathered}J=\beta_1(L+\beta_2\cdot(\omega-\omega_c)+\beta_3\cdot u_v)，\\ u_v=\max(u-u_c^{\min},u_c^{\max}-u)。\\ \end{gathered}$

(4)

式中：$u$为航行器首向速度；$\omega$为首向角速度；$L$为轨迹总长度。

为求解该多目标优化问题，同时考虑算法的实时性、便于后期部署到实际UUV载体上，本文采取粒子群优化算法生成若干必达控制点，再利用B样条曲线拟合方式，使得优化算法与轨迹拟合算法相结合，求解式(4)的优化问题，离线生成UUV群组的可行轨迹。

3.2 PSO与B样条曲线轨迹优化

为了生成满足式(3)条件的UUV可行轨迹，同时考虑到算法执行效率的问题，选择粒子群优化的方法确定轨迹控制点，并结合曲线拟合方法形成若干离散轨迹点。

该方法的核心在于轨迹曲线中的离散控制点选择以及轨迹平滑方法的确定。其中轨迹控制点的选择问题已转化为求解式(4)的优化问题，而常见的轨迹平滑方法包括贝塞尔曲线^[13]、B样条曲线^[14]等方法，由于贝塞尔曲线存在灵活性差的缺点，因此选择B样条曲线方式来拟合轨迹，使其满足式(3)的约束条件，优化算法的执行流程如图5所示。

设有${P_0},{P_1},...{P_n}$一共n+1个控制点，粒子群优化输出的轨迹控制点可定义样条曲线的走向、界限范围，改变轨迹对应的速度、航向、航向角速度等运动学参数。$k$阶B样条曲线的定义，如式(5)所示，基函数定义如式(6)所示。

$P(u) = \sum\limits_{i = 0}^N {{P_i}{B_{i,k}}(u)}，$

(5)

${B_{i,k}}(u) = \frac{{u - {u_i}}}{{{u_{i + k - 1}} - {u_i}}}{B_{i,k - 1}}(u) + \frac{{{u_{i + k}} - u}}{{{u_{i + k}} - {u_{i + 1}}}}{B_{i + 1,k - 1}}(u)。$

(6)

由于需要保证轨迹的起点和终点方向满足特定方向的要求，需满足到达终点时的航向约束要求，且满足航行器的运动学约束。因此选择三次准均匀B样条曲线保证轨迹的二阶导数连续，平滑性好，保证起点和终点方向的相切。由此，得到基于粒子群优化产生的控制点与B样条拟合的可行轨迹。

3.3 基于虚拟点视线角的轨迹跟踪

采用结合虚拟点与视线角制导^[15]的方法实现UUV的轨迹跟踪，算法原理示意如图6所示。图中三角形代表UUV出发时的位置，五角星为UUV终点所在位置以及上层任务指令中所规定的终点航向角度约束。由前面所提出的粒子群寻优得出圆圈所示B样条控制点，再利用三次准均匀B样条插值法拟合出UUV可行轨迹如黑色虚线所示。

考虑到轨迹跟踪问题不同于路径跟踪问题，UUV在控制过程中存在时间约束，速度不恒定。因此，本文采用结合虚拟点控制的视线角制导跟踪方法来解决UUV的动态轨迹跟踪问题。

具体方法是根据UUV跟踪轨迹所经历时间，选择期望轨迹上的虚拟目标点，由此将UUV的轨迹跟踪目标转换为虚拟目标点与轨迹的切线航路跟踪。

根据虚拟目标点与期望航路角，可以得出UUV与期望航路的偏距${y_e}$，则UUV的期望航向可表达为：

${\varphi ^d} = ps{i^d} + \arctan \left(\frac{{{y_e}}}{\vartriangle }\right)。$

(7)

其中：$ps{i^d}$为期望航路角；$\vartriangle$为前视距离。

航向控制误差为${e_\varphi } = \varphi - {\varphi ^d}$，速度误差${e_u} = u - {u^d}$，其中${u^d}$根据UUV跟踪轨迹所经历的时间获取。之后，设计PID控制器如式(8)、式(9)，实现UUV的轨迹跟踪控制。

其中航速控制上，根据期望航速确定一个基准推力，并根据实时的速度误差，修正实际推力，避免UUV推进转速的频繁变化。

${T_u} = {K_{pu}}\left({e_u} + \frac{1}{{{T_{iu}}}}\int {{e_u}{\mathrm{d}}t} + {T_{du}}\frac{{{\mathrm{d}}{e_u}}}{{{\mathrm{d}}t}}\right) + T_u^d，$

(8)

${T_r} = {K_{pr}}\left({e_\varphi } + \frac{1}{{{T_{ir}}}}\int {{e_\varphi }{\mathrm{d}}t} + {T_{dr}}\frac{{{\mathrm{d}}{e_\varphi }}}{{{\mathrm{d}}t}}\right)。$

(9)

3.4 基于势场法的UUV动态避碰

考虑到多UUV在执行任务的过程中，邻居节点出现在避碰域范围内或存在未知障碍物的情况，导致UUV集群系统存在碰撞风险的问题，基于势场控制理论，采用在线动态规划方式实现多UUV的协同避碰。

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

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

(10)

其中：$i = 1,2,3...N$，$N$为避碰域范围内UUV数目之和；${{\boldsymbol{r}}_i} = ({x_i},{y_i})$为UUV的位置矢量；${{\boldsymbol{v}}_i}$为速度矢量；${{\boldsymbol{u}}_i}$为UUV动态规划层的控制输入。

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

$\left\{ \begin{gathered} {\alpha _{\text{1}}}\left(\ln ({r_{ij}}) + \frac{{{R^c}}}{{{r_{ij}}}}\right)，{\text{ }}0 < {r_{ij}} < d，\\ {\alpha _{\text{1}}}\left(\ln ({R^c}) + \frac{{{R^c}}}{d}\right)，{\text{ }}{r_{ij}} > d。\\ \end{gathered} \right.$

(11)

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

同理设计避碰域内UUV与障碍物之间的位置势函数如下式：

$\left\{ \begin{gathered} {\alpha _{\text{2}}}\left(\ln ({r_i}) + \frac{{{h_0}}}{{{r_i}}}\right)，{\text{ }}0 < {r_i} < d，\\ {\alpha _2}\left(\ln (d) + \frac{{{h_0}}}{d}\right)，{\text{ }}{r_i} > d。\\ \end{gathered} \right.$

(12)

其中：${r_i}$为UUV与障碍物的相对距离；${\alpha _{\text{2}}}$为大于零的势场系数；当${r_i} < {h_0}$时，UUV与静态障碍物之间产生斥力；${h_0}$为避碰域值，实际应用中可以根据UUV具体操纵性能选择合适的避碰阈值。

定义UUV与虚拟控制点的速度势场力如式(13)，并设计动态避碰控制律如式(14)所示。

$f_v=-{\beta }({\mathrm{v}}_{{i}}-{\mathrm{v}}_{{h}}) {f_v} = - \beta ({v_i} - {v_h})，$

(13)

${u_i} = - \sum\limits_{\begin{subarray}{l} j \in {\boldsymbol{N}_i} \\j \ne i \end{subarray}} {{\nabla _{{r_{ij}}}}{V_{ij}}({r_{ij}})} - {\nabla _{{r_i}}}{V_h}({r_i}) - {f_v}，$

(14)

其中，${\boldsymbol{N}_i}$为邻接矩阵；${V_{ij}}$为UUV节点间位置势函数；${V_h}$为UUV与静态障碍物间的位置势函数。

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

$\Phi = \frac{1}{2}\sum\limits_{i = 1}^N {\left(\dot r_i^T{{\dot r}_i} + {\sum\limits_{\begin{subarray}{l} j \in {\boldsymbol{N}_i} \\j \ne i \end{subarray}}} {V_{ij}}({r_{ij}}) + 2{V_h}({r_i})\right)}。$

(15)

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

$\dot \Phi = \sum\limits_{i = 1}^N {\dot r_i^T\left({u_i} + \sum\limits_{\begin{subarray}{l} j \in {\boldsymbol{N}_i} \\j \ne i \end{subarray}} {\nabla {V_{ij}}({r_{ij}}) + \nabla {V_h}} ({r_i})\right)}，$

(16)

联立控制律式（14），可得：

$\dot \Phi = - \beta \left(\sum\limits_{i = 1}^N {\dot r_i^T{{\dot r}_i}} \right) \leqslant 0。$

(17)

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

4 仿真效果

为了验证算法的有效性，开展基于Matlab R2018b的仿真研究，构建了包含UUV动力学模型的仿真平台，仿真条件设置如下：

5台UUV初始位置随机分布在一个长50 m，宽50 m的正方形区域内，以设定的目标位置与航线角度，同时到达。

5台UUV根据初始化的位置与终点目标位置，采用KM最优匹配算法得到每台UUV对应的目标占位点。再通过PSO与B样条结合生成可行轨迹，之后采用基于虚拟点视线角制导的轨迹跟踪策略实现多UUV同时到达控制。仿真验证结果如图7～图9所示。

图7为5台UUV在本文所提出算法框架控制下实现同时到达的控制效果。5台UUV在待命区域接收到上层指令发布的集结区域位置集合，经过KM最优匹配算法确定对应目标占位，之后调用PSO与B样条曲线拟合算法实现UUV轨迹规划，再由虚拟点视线角制导律控制算法实现UUV的轨迹跟踪控制，并利用势场法与一致性控制理论避开环境中的静态障碍物，完成群组同时到达指定集结区域的任务。

图8为5台UUV的航向变化曲线，可以看出5台UUV最终航向趋于目标航向，实现同时并按照目标期望航向约束条件到达目标占位点的协同任务，说明了所提出的UUV协同控制算法框架的有效性。

5 结　语

1）本文采用了集中式目标分配、分布式控制执行的协同框架，将任务分配、轨迹规划与跟踪控制分层，解决了多UUV协同过程中采用完全分布式效率低、通信不可靠、全局共识信息难获取以及采用单一集中式控制过于依赖中心节点的问题。

2）本文将人工势场与一致性控制理论引入到UUV协同控制中，可在离线轨迹规划基础上实现在线重规划，完成UUV群组的动态避障。

3）本文采用了PSO与B样条插值相结合的方法完成UUV轨迹规划，并基于虚拟点视线角制导方法实现轨迹跟踪，算法复杂度低、执行效率高，便于工程实现，可以解决多UUV协同控制问题，对实践应用具有一定的指导意义。