基于IAPF算法的无人艇动态编队自主路径规划*

刘梦佳 冯 辉 徐海祥

(武汉理工大学交通学院1) 武汉 430063) (高性能船舶技术教育部重点实验室2) 武汉 430063)

0 引 言

多无人艇编队(unmanned surface vehicle formation,USV Formation)系统已成为无人艇领域的研究热点.相较于单艇作业，无人艇编队具有信息处理能力强、负载能力高、覆盖范围大等优势，在应急救援、水质采样、气象监测、信息侦查等领域有广阔的应用前景[1].国内外对编队控制进行广泛的研究，编队模型控制的主要方法有领航者-跟随者(leader-follower,L-F)法、基于行为(behavior-based)法、虚拟领航(virtual leader)法和基于图论法等[2]，每种方法都有着优缺点，因此，队形控制目前还没有形成统一有效的算法.

L-F法因其简单性和良好的扩展性成为无人艇编队控制领域的研究热点[3-4]，编队路径规划见图1.

图1 编队路径规划示意图

对领航艇进行路径规划，根据领航艇的位置和艏向信息计算出跟随艇的位置信息，从而完成整体编队系统的路径规划.Liu等[5-6]采用快速行进法，采用L-F编队控制，解决了USV编队路径规划问题.张瑞雷[7]提出一种多机器人动态协调控制算法．建立跟随机器人对虚拟机器人轨迹跟踪系统模型.Ali等[8]采用势场法对L-F编队系统进行了机器人路径规划实验，验证了势场法的可行性.吴垠[9]采用L-F法对多智能体编队控制问题进行了研究，设计了一种能够实现其稳定编队的迭代学习控制算法.

人工势场法(artificial potential field,APF)将障碍物和目标假设为相互作用的静电荷进行建模，从而在无人艇周围形成一个潜在的势场.人工势场法的计算简单和实时性好，但主要缺点是尤其是障碍物的坐标未知情况下，APF算法可能存极小值即在局部存在死锁的位置，阻碍无人艇到达目标.为了克服这些局限性，Adeli等[10]将工作空间离散化为矩形网格，Zhang[11]针对凸障碍或凹障碍环境提出了一种改进的随墙方法，叶钱波[12]利用松弛变量将约束优化问题转化为无约束优化问题.

为适用于无人艇的动态运动，本文在对已有的方法进行深入分析的基础上，对传统的人工势场法改进得到IAPF(Improved APF)算法，并且在国内外研究的基础上，本文针对无人艇编队通过狭窄水域的问题，提出自适应编队控制策略，自适应调整队形.

1 无人艇编队模型

1.1 单艇运动模型

对于无人艇的路径规划问题，只研究三个自由度的平面运动，即纵荡、横荡和首摇运动.单艇三自由度运动模型见图2.

图2 单艇三自由度运动模型ψ-首向角；u-船舶纵向线速度；v-横向运动线速度；r-角速度；V-无人艇运动速度；G-重心.

1.2 编队运动模型

采用L-F结构作为编队运动控制模型，无人艇编队运动模型示意图见图3.采用L-F编队结构进行计算时需要参考至少三条艇之间的相对距离和相对方位信息.假设以三条艇的编队控制为例.三条艇之间的关系为，一条领航艇和两条跟随艇，这种方法只需要对两条跟随艇到领航艇之间的期望和实际距离信息ld和l进行控制，保持艇体间相对距离不变就可以达到控制编队队形的目的.

图3 编队运动模型L1，L2-跟随艇USV1和跟随艇USV2与领航艇USVL之间的相对距离；ψ1，ψ2-两跟随艇的首向角

只要将领航艇与跟随艇之间的实际相对距离L与期望相对距离LR之差ΔL、实际相对角度φ与期望角度φR之差Δφ保持在允许的误差范围内即可达到队形控制的目的，满足约束条件为

ΔL=‖L-LR‖≤δL

(1)

Δφ=‖φ-φR‖≤δφ

(2)

标准的L-F编队控制结构的队形稳定性高，但不利于动态调整编队中跟随艇的位置，所以本文提出虚拟跟随者的编队控制结构，具体是设置一艘虚拟跟随艇USVV，与另一艘跟随艇USV2保持相同的期望相对距离和期望角度，跟随艇USV1的位置和角度可以根据虚拟跟随艇的位置推算出.虚拟跟随者的L-F具体示意图见图4.

图4 虚拟跟随者-编队运动模型

由图4可知，无人艇USV1，USV2是USVL的跟随无人艇，USVV是无人艇USV1需要跟随的虚拟无人艇，与USVL保持期望距离LV和期望角度φV，根据USVL的位置和首向角即可得到USVV的位置和首向角.

为达到所需的编队控制结构，假设φ1=0°，各跟随艇的位置计算为

φV=φ2

(3)

VV=VL

(4)

xV=xL-LVsin (ψ+φV)

(5)

yV=yL+LVcos (ψ+φV)

(6)

x2=xL-L2sin (ψ-φ2)

(7)

y2=yL-L2cos (ψ-φ2)

(8)

x1=xV-L1sinψ

(9)

y1=yV-L1cosψ

(10)

2 自适应编队控制策略

2.1 自适应调整跟随角

在无人艇编队进行自主避障时，需保证跟随艇USV1和USV2避免碰撞到障碍物，对两艇跟随角进行自适应调整策略，当两跟随艇分别距左、右障碍物的距离小于安全距离时，根据其可通航水域宽度自适应调整两艇跟随角以避开障碍物，具体计算为

(11)

(12)

式中：φ1为跟随艇USV1跟随角；φ2为跟随艇USV2跟随角；B1为跟随艇USV1距左侧障碍物的距离；B2为跟随艇USV2距右侧障碍物的距离.

2.2 自适应队形控制

在自适应调整跟随角的基础上，针对通航水域较窄的情况提出自适应队形控制，具体策略为将无人艇编队看作一个整体，在宽阔水域，按照前文提出的虚拟跟随艇L-F编队控制策略进行航行，在狭窄水域，根据水域宽度自适应调整编队整体宽度，即自适应调整φ，为

(13)

(14)

式中：B为可通航水域宽度.

当可通航水域宽度只能通过一艘艇时，即φV=φ2=0°，领航艇与跟随艇保持一字队形通过狭窄水域，见图5.

图5 自适应调整编队结构示意图

3 IAPF算法

3.1 基本原理

APF基本思想是在环境中的无人艇，对目标位置构造引力势场和静态障碍物周围构造斥力势场.引力吸引无人艇朝目标点方向运动，斥力阻止无人艇向障碍物方向运动，在这两个势场力共同作用下使无人艇向目标运动.

定义S空间内，无人艇位置r(x1,y1)，目标位置g(x2,y2)，无人艇到静态障碍物的最近位姿点oi(x3,y3)，环境中目标对无人艇产生的引力场Uatt的吸引力Fatt，静态障碍物对无人艇产生的斥力场Urep的斥力Frep见图6.引力场和目标吸引力为

Uatt(ρ(r,g))=ξ‖ρ(r,g)‖m

(15)

Fatt(ρ(r,g))=-Uatt(ρ(r,g))=

mξ‖r(x1,y1)-g(x2,y2)‖m-1ρ(r,g)

(16)

式中：m值为势场函数曲线的形状；ξ为目标势场系数；ρ(r,g)为无人艇与目标物距离函数，ρ(r,g)=‖r(x1,y1)-g(x2,y2)‖；Uatt(ρ(r,g))为Uatt在无人艇位置r(x1,y1)处势场变化率最大的方向；ρ(r,g)为无人艇到目标的单位矢量.

图6 二维环境S中无人艇的引力和斥力

静态障碍物对无人艇产生的斥力场及相对应的斥力为

(17)

Frep(ρ1(r,o))=-Ureps(ρi(r,o))=

(18)

式中：ρ0为障碍物的斥力影响距离，只有在ρ0的范围之内无人艇才受到障碍物的排斥作用；ρi(r,o)为无人艇与第i个静态障碍物的最小距离，ρi(r,o)=‖r(x1,y1)-oi(x3,y3)‖；ρi(r,o)是由oi(x3,y3)指向r(x1,y1)的单位向量.

传统的人工势场法势场参数取值不易选取，且生成的路径容易产生局部极小值，局部震荡.同时未考虑到动态障碍物的情况.

3.2 改进方法

3.2.1改进引力场

改进后的引力场函数包含了无人艇与目标物的相对位置、相对速度和相对加速度产生的势场，改进后的引力场计算公式为

Uatt(q,v,a)=αq(ρ(r,g))m+

αv‖v‖n+αa‖a‖p

(19)

式中：αq，αv，αa和m，n，q为比例系数，不同的值表示在引力函数中无人艇和目标点相对位置、速度和加速度信息的权重.由引力场可推得相应的吸引力大小为

Fatt(q,v,a)=-Uatt(q,v,a)=

(20)

Fatt(q)=-∂Uatt(q,v,a)/∂q=

mαq(ρ(r,g))m-1ρ(r,g)

(21)

Fatt(v)=-∂Uatt(q,v,a)/∂v=nαv‖v‖n-1v

(22)

Fatt(a)=-∂Uatt(q,v,a)/∂a=pαa‖a‖p-1a

(23)

势场引力可表达为

Fatt(q,v,a)=Fattq(q)+Fattv(v)+Fatta(a)

(24)

图7 无人艇所受势场引力合力示意图

3.2.2改进斥力场

(25)

(26)

算法流程见图8.

4 仿真结果分析

采用的仿真平台是MATLAB，仿真是基于二值化图像且不考虑风、浪、流作用下的无人艇编队路径规划，仿真图像像素大小为500×500，为便于分析仿真结果，表1～2给出了船舶参数和算法参数.

表1 船舶参数

表2 算法参数

为便于对比分析仿真结果，本文首先对传统的人工势场法与改进后的人工势场法进行算法的性能对比；然后对比应用于无人艇编队的效果，对比虚拟跟随者L-F与标准的L-F效果；最后验证提出的自适应编队控制策略，证明其可行性.

4.1 算法性能对比仿真结果

为对比IAPF算法与标准的APF算法性能，将标准的L-F无人艇编队应用于提出的IAPF算法进行避障仿真，仿真对比结果见图9.APF和IAPF算法采用相同的参数：引力参数2，斥力参数30.由图9仿真结果可知，改进后的算法IAPF明显优于标准APF算法.

图9 APF与IAPF算法对比

4.2 改进的控制策略仿真结果

为验证本文提出的虚拟跟随者L-F编队控制算法的动态调整性能，与标准L-F控制结构进行对比仿真，采用提出的IAPF算法，表3给出了编队仿真参数，仿真结果见图10.由图10b)可知，增加了虚拟跟随艇后，跟随艇1与跟随艇2之间的纵向距离增大，保证了两艇之间的安全，这种控制结构有利于实现本文提出的自适应编队控制策略.

图10 编队效果对比

L1/mL2/mLV/mφ1φ2φV404040π/6π/60

4.3 自适应编队控制策略仿真结果

为保证无人艇编队整体避开静态障碍物，并且保证跟随艇之间的安全，对本文提出的自适应编队控制策略进行仿真验证，跟随艇USV1与虚拟跟随艇USV virtual之间的跟随角设为15°，结果见图11～12.

图12 跟随角对比结果

由图11a)可知，跟随艇1与跟随艇2均在避障过程中撞到障碍物上，未能安全航行.由图11b)可知，采用自适应调整跟随角策略，实现在障碍物附近自动调整角度以避开障碍物，保证跟随艇安全航行.

图13为采用自适应调整编队宽度的策略以通过限制水域的仿真结果，由图13可知，采用自适应编队宽度控制策略后，无人艇编队可以实现自适应调整跟随角大小，保证安全通过限制水域.跟随艇的跟随角变化情况见图14，在通过限制水域时，跟随角明显根据通航水域宽度进行调整.

图13 自适应调整编队宽度仿真对比结果

图14 跟随角对比结果

5 结 束 语

文中采用虚拟跟随艇建立Leader-Follower无人艇编队模型，提出自适应编队控制策略，可根据通航水域宽度和离障碍物的距离进行自适应调整跟随角，实现不同环境条件下的队形调整，保证无人艇编队的整体安全性.针对编队路径规划提出IAPF算法，引入实际船舶在水域中的速度、加速度参量，改进了传统势场的势场力.仿真结果表明，在狭窄水域，无人艇自适应编队控制策略可以实现动态编队，实现跟随艇避开障碍物以及通过狭窄水域，所提出的编队模型也适用于动态调整编队结构.