0 引　言

目前智能体协同控制成为国内外研究的热点之一，协同控制技术被应用于许多领域^[1-2]，智能体编队协同控制技术取得了大量科研成果，如通过控制多个机器人以特定方式自动运输各种货物；如多个自主运动小车被广泛应用于军事边防编队巡逻、资源地理勘测、侦察救援、扫雷或卫星的姿态运动协调及空间探测；如在警务领域的多个移动机器人编队成一个弧形包围网抓捕入侵者等，形成了以人工势场法^[3-5]、虚拟结构法^[6-7]、基于行为控制法^[8]、跟随领航者法为主要代表的多种编队控制方法。

而在UUV集群编队航行控制中，利用人工势场法实现了集群无碰队形变换技术，虽然也实现了集群的避障和避碰功能，由于人工势场法是通过引入与障碍物间的斥力来防止碰撞，容易产生局部最小值而造成无法运动到终点结果^[9]，导致避障方法的实时性差问题。同时在障碍物分布稀松的情况下，由于多个候选方向造成最优路径丢失现象，导致避障方法的自主性差问题。

针对UUV集群编队航行的安全性需求，本文在人工势场法^[10]（Artificial Potential Field，APF）完成编队控制基础上，引进圆形扩张法（Circle Sector Expansions，CSE+）和鸽子群优化算法（pigeon-inspired optimization，PIO）寻找下一时刻位置和方向，解决实时避障问题。

1 理论基础 1.1 CSE+方法

CSE+方法的基本思想为进行圆的扩张找到障碍物，如图1所示。在由圆形障碍物随机填充的环境中，找到到达目标位置的可行路径，首先在图1（a）中，智能体从起点开始膨胀寻找障碍物，其中细箭头转向为圆的膨胀方向。然后在图1（b）中，沿与障碍物1的圆心方向膨胀确定障碍物2。再在图1（c）中，穿过障碍物1和障碍物2构成的通道，继续向前膨胀确定障碍物3。最后在图1（d）中，根据与目标的方位角选择向左或向右运动方向，重复这一过程而不断接近目标位置。

CSE+方法在障碍物分布密集情况下，利用根据Apollonius相切问题确定相切圆，移动智能体运动到相切圆圆心为下一步位置，但对于障碍物分布稀疏情况时，确定下一步位置效果很差。

1.2 PIO方法

PIO方法通过建立地图和指南针算子以及地标算子，（见图2），在安全范围内寻找下一目标位置，而后进行转向判断。

首先利用地图通过膨胀寻找新的障碍物，然后建立指南针算子以及地标算子，不断重复这一过程，最后直到智能体运动到达终点。PIO方法较好地解决了UUV集群在稀疏环境空间中的实时避障算法设计^[9,11]。

1.3 APF方法

APF方法通过智能体个体之间引力、斥力求得合力，就是“远吸近斥”原理控制智能体的位置和方向。

首先对UUV集群编队建立Leader-Follower模型，完成初始化队形，如图3（a）和3（b）所示。当UUV间的距离较远时，吸引力发挥作用，缩编队队形。当UUV间的距离较近时，排斥力发挥作用，稀疏编队队形。在UUV间的距离合理范畴内时，不受任何力的影响，实现UUV编队无碰队形变换和保持。

2 算法设计 2.1 UUV集群无碰撞编队算法设计

步骤1　给定当前Learder及状态 $({x_1},{v_1})$ ；

步骤2　定义拓扑结构，求得受控UUV集合 ${N_i}$ ；

步骤3　受控上UUV集合 ${N_i}$ 的下一时刻状态输出 $ ({X}^{{N}^{i}},{v}^{{N}^{i}})$ ，得下一时刻Follower的理想控制输入 $(V_C^i,\psi _C^i,h_C^i)$ ；

步骤4　由型式得下一时刻Follower的的实际状态 $(x_{}^i,{\rm{y}}_{}^i,h_{}^i,V_{}^i,\psi _{}^i,{\lambda ^i})$ ；

步骤5　状态转换为 $ ({X}^{i},{v}^{i})$ ，并作为模型输入；

步骤6　返回步骤1，直至达到仿真终了条件。

2.2 UUV集群实时避障算法设计

步骤1　从起点出发向前膨胀直到找到障碍物1，如图1（a）所示。沿障碍物1与起点方向延长线进行膨胀确定障碍物2，如图1（b）所示。若构成的通道不可通行，即 ${d_{12}}\langle {d_w}$ ，调整膨胀方向重新寻找障碍物2。确定障碍物1和障碍物2后执行步骤2。

步骤2　从障碍物1和障碍物2所构成的通道中穿过进行膨胀寻找障碍物3，如图1（c）所示。确定由障碍物构成的安全范围A的大小。当 ${s_A} < {s_t}$ 时进行步骤4，否则将进行步骤5。

步骤3　经过障碍物1和障碍物2所围成的通道到达与3个障碍物相切的相切圆的圆心，后执行步骤7。

步骤4　初始化鸽群优化算法的参数，如空间维度D、地图罗盘算子R、种群数量 ${N_p}$ 、最大迭代次数 $NC{1_{\max }}$ 和 $NC{2_{\max }}$ ^[12]，并确定鸽群优化算法的适应度值计算函数 $threat - count$ ，随机初始化鸽群的速度与位置，将执行步骤5。

步骤5　以安全范围重心作为现有最优结果，运算地图罗盘算子，多次迭代更新（直到迭代次数 $N{C_1} = NC{1_{\max }}$ ）鸽群的速度V与路径X。根据适应度值fitness的大小确定每只鸽子的最优位置 ${X_p}$ ，以及全局最优位置 ${X_R}$ ^[13-17]。

步骤6　根据适应度值淘汰20%的鸽子，再利用地标算子进行迭代更新直到迭代次数 $N{C_2} = NC{2_{\max }}$ ，调整鸽群的速度V与路径X，以当前全局最优X作为下一位置，并移动到这一位置，而后执行步骤7。

步骤7　判断是否可直接达到目标位置，不可以时进行步骤9。

步骤8　记录当前位置与障碍物信息至搜索树中。

步骤9　进行转向判断。若可进行转向，选择构成当前通道的2个障碍物作为新的障碍1和障碍2，并进行步骤3；若不可转向，将进行步骤10。

步骤10　标记当前通道不可行，机器人掉头返回上一层搜索树，选择另一条通道，若仍不可行，则机器人再一次掉头并返回搜索树更上一层再次重新执行步骤10；若可行将进行步骤2。

3 测试与仿真 3.1 UUV集群无碰撞编队算法Matlab仿真实验

首先完成4个Follower和1个Leader初始化，即位置pose，速度V，加速度控制量control三个量，然后设置Follower相对Leader的位置，再设置位置偏差线速度调节5、位置偏差角速度调节5、朝向偏差角速度调节0，效果如图4所示。

3.2 UUV集群实时避障算法Matlab仿真实验

1）障碍物分布稀疏情况下的APF方法避障实验

参数设置为计算引力需要的增益系数1，计算斥力的增益系数2，障碍影响距离100，当障碍和车的距离大于这个距离时，斥力为0，即不受该障碍的影响，障碍个数8，步长5，迭代次数200，UUV个数为4，效果如图5（a）所示。

2）障碍物分布密集情况下的CSE+方法避障实验

在PIO参数设置为地磁导航次数90，地标导航次数15，种群数30，维数2维，地磁因数0.3，经过28次迭代，UUV完成避障进入稳定状态，效果如图5（b）所示。

4 结　语

本文完成UUV编队无碰队形变换技术和UUV编队实时自主避障技术的设计与仿真，达到集群编队的解散和恢复，100%实现原始队形的无碰撞保持。本算法在ROBO-MAS即桌面微型移动机器人系统进行测试，系统的测试环境为射频通信芯片进行组网，最高传输速率可达2 Mbps，在小型实验场地内通过光电投影定位技术，实现机器人位置感知和数据通信，完成本文算法的测试，效果达到预期效果。

本文实时避障算法可以应用在蜂群UUV作战中，在水下通信延迟与丢包状态下，UUV集群编队实现无碰保持与变换效果。