波浪滑翔器编队避障方法

王震， 桑宏强， 孙秀军，2，3*

(1. 天津工业大学机械工程学院， 天津 300387; 2. 中国海洋大学物理海洋教育部重点实验室， 青岛 266100; 3. 青岛海洋科学与技术国家实验室海洋动力过程与气候功能实验室， 青岛 266237)

波浪滑翔器(wave glider， WG)作为一种新型的海洋探测平台[1]，通过波浪能产生向前推力，同时携带太阳能电池板，为长期的海洋观测任务提供了保障。波浪滑翔器具有续航时间长、控制容易等优点。然而单台波浪滑翔器观测区域有限，无法实现大范围的海洋数据采集，因此需要多台波浪滑翔器组成编队来完成更加复杂的任务。波浪滑翔器编队如何防止内部的碰撞，又能安全躲避障碍物，最终顺利地完成一定区域内的观测任务，显得尤为重要。

为了实现期望的编队行为[2-3]，学者们提出了多种思路，按照编队控制思想可分为领航跟随者法[4]、虚拟结构法、基于一致性算法、基于行为法[5]、基于势函数法[6-7]和图论法[8]等。Cui等[9]应用参考路径和虚拟水下航行器的思想，同时通过反步法来控制编队队形。潘无为等[10]通过结合人工势场法和虚拟结构法，实现了智能水下机器人的编队控制。Choi等[11]则应用自适应技术通过对领航者的速度进行估计，然后利用位置信息完成编队控制。而针对避障问题，常见的避障控制算法包括遗传算法、动态窗口法、人工势场法等。许多研究人员采用人工势场法来实现避障控制，因为该方法具有计算简单、避障安全性高、实时性强等优点。韩君[12]通过将北斗定位信息和全球定位系统(global positioning system，GPS)定位信息相融合，同时结合人工势场法，实现了无人船的高精度自动化智能航行。Wang等[13]提出了一种基于人工势场算法的波浪滑翔器避障方法，通过引入一个新的斥力函数解决了局部最小域问题，实现了波浪滑翔器避障，但未考虑编队问题。张强等[14]通过结合蚁群算法与人工势场法，解决了自主机器人的路径规划问题。Zhang等[15]通过增设虚拟目标点的方法，解决了局部最小域问题，实现了无人机的编队避障控制。

由上述分析可知，虽然大多数改进的人工势场法解决了局部最小域问题，但现有的研究主要集中在机器人个体的控制上，未考虑编队内部的碰撞及编队的保持问题，同时研究对象大多具有一定驱动能力的自主水下机器人，针对波浪滑翔器这种欠驱动无人船的研究较少。针对上述问题，现通过结合领航跟随者法与人工势场法，将波浪滑翔器编队视为多体系统，利用人工势场法实现编队的避障以及编队内部的防撞与保持，为波浪滑翔器编队的应用提供理论研究基础。

1 波浪滑翔器编队策略

1.1 波浪滑翔器动力学建模

准确的波浪滑翔器运动学模型，有利于分析和控制波浪滑翔器编队和避障控制系统。由于海洋环境多变，所以波浪滑翔器的运动情况较为复杂。为了保证编队避障方法研究的可靠性，对波浪滑翔器运动模型做出如下的简化。

(1)因为水下牵引机在水下足够重，无论是浮体船随波浪上升阶段还是下降阶段，脐带缆始终处于拉紧状态。因此，在将脐带缆视为刚体的前提下，推导了波浪滑翔器的运动参数。

(2)当考虑波浪滑翔器的整体运动时，水面浮体船和水下牵引机的形状忽略不计。

(3)波浪滑翔器的质量分布均匀，且原点位于重心。

所研究的波浪滑翔器主要是在海平面上运动，只考虑水面浮体船的横荡、纵荡、垂荡和艏摇4个自由度就可以模拟出波浪滑翔器的运动状态。因此基于大地坐标系(E)和船体坐标系(F)(图1)，建立波浪滑翔器运动学模型为

(1)

(2)

图1 坐标系建立Fig.1 Establishment of coordinate system

1.2 领航跟随者编队策略

采用领航跟随者方法控制波浪滑翔器的编队。领航跟随者编队策略中需要设定一台波浪滑翔器为领航者，其余波浪滑翔器作为跟随者，该编队策略下的整体编队行动都是基于领航波浪滑翔器的行动轨迹，领航跟随者编队策略示意图如图2所示。

图2 领航跟随者编队示意图Fig.2 The formation schematic of the leader and followers

1.3 间距势能场编队保持策略

由多个波浪滑翔器组成的集群网络，可以通过编队协同控制共同完成单台波浪滑翔器无法完成的复杂海洋任务，同时降低任务对单台设备性能的要求及波浪滑翔器的制造成本。波浪滑翔器的协同控制主要体现在对多台波浪滑翔器进行整体编队和安全避障。

现有的多机器人编队系统绝大多数是通过控制单体的各种局部行为，从而实现整体编队系统的协同工作。在基于领航跟随者编队策略的基础上，根据波浪滑翔器编队的特殊海洋工作环境，考虑到各波浪滑翔器单体间信息异步传输的复杂性，以及各个参数获取的不确定性等因素，引入人工势场算法的思想，在波浪滑翔器编队间建立间距势能场，利用间距势能场所产生的作用力使波浪滑翔器个体之间形成互联，成为一个“虚拟的多波浪滑翔器系统”，最终实现编队队形的保持。

在相邻两波浪滑翔器间建立间距势能场，来控制相邻两波浪滑翔器的间距，使它们保持在一定的距离位置上。定义间距势能函数为

(3)

式(3)中：rij为相邻波浪滑翔器之间的距离；d0为相邻波浪滑翔器间的最小期望距离；d1为相邻波浪滑翔器发生吸引作用还是排斥作用的临界距离；kI为间距势能场系数。

当相邻波浪滑翔器间距rijd0时，间距势能场会呈现吸引势场，使得相邻波浪滑翔器互相吸引回到相应的位置，保证编队队形的稳定性。三角形波浪滑翔器编队示意图如图3所示。

间距势能场产生的作用力为它的负梯度，可表示为

图3 三角形波浪滑翔器编队Fig.3 Triangular formation of wave gliders

(4)

2 波浪滑翔器避障策略

2.1 人工势场法

人工势场法具有计算简单、实时性好等特点，采用人工势场法实现波浪滑翔器的编队控制，为了方便研究，障碍物都近似为圆形。

人工势场法通过势场函数来表达波浪滑翔器、目标点以及障碍物之间的约束关系，势场函数的负梯度即为相应的受力表达式。传统人工势场法中引力势场的数学模型和斥力势场的数学模型分别表示为

(5)

(6)

式中：(x，y)为波浪滑翔器的位置坐标；δ为吸引力系数；ρgoal为波浪滑翔器与目标点之间的距离；η为排斥力系数；ρob为波浪滑翔器与障碍物之间的距离；ρ0为障碍物的影响距离，意味着只有波浪滑翔器在障碍物影响距离内才会受到障碍物的排斥力。

2.2 局部最小域问题

人工势场法实现波浪滑翔器编队避障的过程中通常存在局部最小域问题，表现为引力和排斥力的合力为零。满足以下任意条件则波浪滑翔器陷入局部最小域。

条件(1)：Uatt(x)+Urep(x)+UI(x)=0。

条件(2)：|xa-xb|≤αsab。

条件(1)成立表示波浪滑翔器的合势能为零，其中，Uatt为目标点的引力势能，Urep为障碍物的排斥势能，UI为相邻波浪滑翔器的间距势能。条件(2)成立表示波浪滑翔器移动了较长距离，但是位移很小，|xa-xb|表示两点之间的位移大小，sab表示两点间的路程，α是一个很小的正数。

针对上述局部最小域问题，通过引入一个沿障碍物切线方向的引力Uatt2来帮助波浪滑翔器编队逃出局部最小域，如图4所示。

根据几何知识，从波浪滑翔器的位置(x，y)到圆心为(xo，yo)半径为r的障碍物可做一条切线，切线的斜率可以表示为

图4 切线方向的新输入引力Fig.4 The new input attracted force along tangent direction

(7)

切点的坐标可以表示为

(8)

最终，沿切线方向的新输入引力Uatt2的大小可以表示为

(9)

3 波浪滑翔器编队避障策略

浪滑翔器编队的控制体系主要分为3个层次，分别是任务协调层、任务执行层和编队避障层，如图5所示。任务协调层的主要任务，即所有波浪滑翔器控制系统初始化，主要包括编队队形信息初始化操作、编队任务分配初始化、设备搭载传感器初始化操作。任务执行层根据分配好的任务需求同时参考搭载传感器获得的环境信息，按照既定的虚拟领航者编队方式行进同时通过间距势能场实现编队队形保持。编队避障层在波浪滑翔器编队行进的过程中，根据改进人工势场法对障碍物进行躲避，保证波浪滑翔器编队的安全。

图5 波浪滑翔器编队控制体系Fig.5 Control architecture of wave glider formation

4 仿真与结果分析

4.1 波浪滑翔器三角形编队避障仿真

运用所提出的新输入引力的方法给出具有局部最小域下的编队避障仿真。假设由3台波浪滑翔器组成三角形的编队队形，要求波浪滑翔器编队在保持队形的情况下，自主地躲避障碍物抵达目标位置。区域内有一个半径为50 m的圆形障碍物，圆心位于(230，230)m处。相邻两波浪滑翔器之间的期望距离范围为16～18 m。波浪滑翔器的初始位置分别为WG1(50，50)m，WG2(50，30)m和WG3(30，50)m，目标点位置为(400，400)m。波浪滑翔器编队避障仿真图如图6所示。编队在(80，80)m处陷入局部最小域，此时引入沿切线方向的新输入引力帮助编队逃离局部最小域。

图7为波浪滑翔器和障碍物之间的距离，可以看出随着波浪滑翔器朝向目标点运动，距离障碍物越来越近并在75 s时距离障碍物最近，但始终与障碍物保持着安全距离，没有发生碰撞。

图8为波浪滑翔器编队内部两两设备之间的距离，可以看出由于受间距势能场和障碍物排斥势场的共同影响，相邻波浪滑翔器之间的距离越来越小，直到75 s时，因为受到障碍物的排斥势场影响最大，此时相邻波浪滑翔器之间的间距最小。当波浪滑翔器编队逐渐远离障碍物后，障碍物的排斥势场影响逐渐变小，此时波浪滑翔器受间距势能场的影响，相邻设备间的间距逐渐恢复到期望范围内。

图6 波浪滑翔器编队运动轨迹Fig.6 Motion trajectories of the wave glider formation

图9为波浪滑翔器编队中各设备的速度图，可以看出各设备的速度保持稳定，所以编队保持较为稳定。

图7 波浪滑翔器和障碍物之间的距离Fig.7 The distances of the wave glider and the obstacle

图8 相邻波浪滑翔器之间的距离Fig.8 The distances among adjacent wave gliders

图9 波浪滑翔器速度Fig.9 The velocities of wave gliders

4.2 有无间距势能场编队仿真对比

波浪滑翔器编队的控制主要是用来调节相邻波浪滑翔器之间的距离。图10用间距势能场实现波浪滑翔器的编队控制，图11的编队避障算法不包括间距势能场的控制，所以两个波浪滑翔器的运动轨迹有重合的部分，表明两设备发生了碰撞。图12给出了无间距势能场的作用下相邻波浪滑翔器之间的距离，可以看出随着编队朝向目标点运动，受障碍物排斥势场的影响，设备间距越来越小，在距离障碍物最近的时候WG1和WG2发生了碰撞，图中的红色实线表明两个波浪滑翔器WG1和WG2之间的距离为零。

图10 有间距势能场的编队轨迹Fig.10 Formation trajectories with spaced potential field

图11 无间距势能场的编队轨迹Fig.11 Formation trajectories without spaced potential field

图12 无间势能场下相邻波浪滑翔器间距Fig.12 The distances among adjacent wave gliders without spaced potential field

5 结论

针对欠驱动波浪滑翔器的编队避障问题，提出了一种基于领航跟随者法和人工势场法的多波浪滑翔器编队避障方法。通过MATLAB仿真实验，得出如下结论。

(1)采用领航跟随法与人工势场法结合的思想，实现了波浪滑翔器编队。通过间距势能场产生的力使各个波浪滑翔器连接成一个构型，同时控制设备的间距，避免编队内部发生碰撞的同时实现了编队队形的保持。

(2)采用人工势场法实现波浪滑翔器编队的避障，针对人工势场法的局部最小域问题，通过引入沿障碍物切线方向的引力，帮助波浪滑翔器打破平衡状态，从而逃脱局部最小域。通过仿真验证了所提波浪滑翔器编队避障方法的有效性。