具有实时避障能力的UUV 编队技术设计与研究

佟盛，李大辉

(1. 中国船舶集团公司第七一四研究所，北京 100101；2. 齐齐哈尔大学，黑龙江 齐齐哈尔 161000)

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）中，根据与目标的方位角选择向左或向右运动方向，重复这一过程而不断接近目标位置。

图 1 CSE+寻径示意图Fig. 1Diagram ofCSE+ method

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

1.2 PIO 方法

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

图 2 PIO 寻径示意图Fig. 2Diagram ofPIO method

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

1.3 APF 方法

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

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

图 3 编队队形模型及恢复示意图Fig. 3The model of cluster keep and resume

2 算法设计

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

步骤1给定当前Learder 及状态 ( x1,v1)；

步骤2定义拓扑结构，求得受控UUV 集合 Ni；

步骤3受控上UUV 集合 Ni的下一时刻状态输出()，得下一时刻Follower 的理想控制输入()；

步骤4由型式得下一时刻Follower 的的实际状态(xi,yi,hi,Vi,ψi,λi)；

步骤5状态转换为 ( Xi,vi)，并作为模型输入；

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

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

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

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

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

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

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

步骤6根据适应度值淘汰20%的鸽子，再利用地标算子进行迭代更新直到迭代次数NC2=NC2max，调整鸽群的速度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）所示。

图 4 无碰撞编队算法Matlab 仿真实验Fig. 4Simulation of the contact-free cluster algorithm

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

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

4 结 语

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

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

图 5 实时避障算法matlab 仿真实验Fig. 5Simulation of real-time obstacle avoidance algorithm