欠驱动无人艇集群协同编队路径规划仿真

王艺凯,孙 俊,王 偲

(江南大学人工智能与计算机学院,江苏 无锡 214122)

1 引言

无人艇(Unmanned Surface Vehicle,USV)是一种具备自主运行能力的水面艇,作为新一代信息技术的产物,无人艇不仅是军事无人作战系统的重要组成部分,而且为提升国家科技创新能力提供了重要支撑[1]。由于海洋环境日益复杂、作业任务日益多样,无人艇系统处理复杂任务往往需要相互间的协作才能更有效率地、更高质量地完成。在海军作战系统中,一艘无人艇可以实现有限的功能,而多艘USV协同编队不仅可以提高效率,还可以完成许多更复杂的任务,如协同防护、编队集结等,因此无人艇集群化必然是无人艇发展的重要方向[2,3]。编队控制问题作为无人艇集群化研究领域的重要课题,引起了国内外众多学者的关注。目前国内外对多智能体集群编队控制问题的研究方法主要有领航者-跟随者(Leader-Follower,L-F)法[4]、基于行为(Behaviour-Based)法[5]、虚拟结构(Virtual Structure)法[6]、人工势场(Artificial Potential Field,APF)法[7]。

领航跟随法的主要实现形式是编队中设置一艘无人艇作为领航者,其余无人艇作为跟随者,跟随者与领航者保持固定的相对位置形成队形[8]。Sun等[9]采用基于有限控制集模型预测控制方法提高了跟随者的自治性。Park等[10]提出一种基于连通性保持和避障性能函数的鲁棒主从编队跟踪设计。领航跟随法的优点是队形稳定性高,容易设计实现,缺点是过度依赖领导者,跟随者反馈差,避障效果差。虚拟结构法要求机器人以刚体上的相应点的位置和姿态信息作为各自的跟踪目标形成队形[11]。Tank等[12]提出了虚结构法,该方法能取得较高的精度和轨迹跟踪效。Ren[13]采用信息一致性算法,提出了一种分布式虚拟结构的多智能体编队算法。虚拟结构法的优点是队形保持性能良好、系统稳定性容易保证,缺点是队形不灵活、避障不易实现。人工势场法是通过设置势场函数,无人艇在势场的吸引力、排斥力作用下,得到相应的队形。Sang等[14]提出在人工势场模型中引入A*算法,生成全局最优路径,降低无人艇陷入局部极小值的概率。人工势场法的缺点在于,队形的变化需要对势场函数的重新配置,因而不够灵活。基于行为控制方法则是将任务整体分解成简单的子任务,系统内的智能体根据自身的性能被分配到不同的子任务,就需要有不同的行为来完成子任务。Tucker 和 Ronald[15]最早采用自下而上构造系统的方式,提出了一种基于行为的控制方法,将队形控制任务分解为一系列的基本行为,通过行为的综合来实现运动控制。基于行为法的优点则是可解决多任务问题、避障效果好,缺点是队形控制的稳定性得不到保证。

本文采用零空间行为法,对欠驱动无人艇集群编队路径规划问题进行研究。零空间行为法的核心是将低级任务向量向高级任务的零空间进行投影,将综合的任务输出作为最终的任务输出,在完成高级任务的同时部分或全部完成低级任务,具有实时性强、任务之间不冲突的优点[16]。

本文在国内外研究的基础上,对整个问题进行任务分解,在任务分解完成后对不同子任务完成任务函数的处理,将虚拟结构法与零空间行为法相结合,保证了无人艇集群编队的稳定性,通过仿真验证所述算法在无人艇集群编队路径规划问题上的可行性。

2 零空间行为法建模

行为法的主要思想是首先设定一些智能体的期望行为,每个行为都有其对应的目标或任务。在欠驱动无人艇集群编队路径规划任务中,零空间行为法首先将任务分解为多个可以独立完成的子任务并分别计算出各个子行为的任务函数。为解决各子任务之间可能存在的任务冲突,首先对各子任务划分优先级,其次将低优先级任务的任务向量向高优先级任务的零空间投影,从而抵消低优先级任务对高优先级任务的任务冲突,最终得到任务综合的输出函数,传输给底层的无人艇运动控制器控制无人艇的运动。

定义σ为要控制的任务变量,则其函数模型可表示为

σ=f(r)

(1)

对应的微分关系为

(2)

图1 无人艇集群运动模型

其中J(r)是r的输出雅可比矩阵。

在无人艇运动过程中通过求取最小范数速度,从而得到最小二乘解

(3)

在任务的完成过程中,无人艇的运动轨迹可以通过vd的时间积分来获得。为消除离散时间积分导致的无人艇坐标位置偏移,可将式(3)改写为如下形式

(4)

由式(4)可知,在多任务的情况下,各任务速度计算公式如下

(5)

其中下标i表示第i个子任务。如果下标也表示任务的优先级(即任务1是最高优先级的任务),在n个子任务的情况下,则式(5)可以被改写为

vd=v1+N1[v2+…+Nn-1vn]

(6)

式(6)的几何解释为每个任务速度的计算是单独进行的,在计算该速度对总的速度贡献之前,用I表示适当维数的单位矩阵,将第i优先级的任务通过式(7)投影到上一优先级任务的零空间上,以抵消该任务对高优先级任务的影响

(7)

一方面,基于零空间的行为控制是在非奇异配置下完成最高优先级的任务。另一方面,较低优先级的任务仅在子空间中完成,在子空间中,它们不与具有较高优先级的任务冲突,也就是说,每个任务达到次优条件,该次优条件根据由最高优先级任务施加的约束来优化任务。

3 任务处理

在欠驱动无人水面艇集群编队任务中,子任务包括以下三项:避障任务、质心任务、队形任务。下面分别对这三项子任务进行任务处理。

3.1 避障任务

避障任务是优先级最高的任务,对于保证无人艇集群的完整性至关重要。欠驱动无人艇集群运动过程中可能遇到两种障碍物第一种为环境中存在的礁石一类集群外障碍物,第二种为集群中除自身外的其它无人艇,因此欠驱动无人艇集群的避障任务不是一项集群任务,而是每艘无人艇的单独任务。

在前进方向存在障碍物时,无人艇避障的目的就是使无人艇与障碍物保持安全距离。因此,避障任务将无人艇——障碍物方向上的速度作为输出,该速度保证无人艇与障碍物之间的距离不小于安全距离。

当无人艇遇到多个障碍物时,将无人艇与障碍物之间距离作为避障优先级排序依据,距离最近的障碍物优先级最高,分别计算无人艇对每个障碍物的避障速度。

设无人艇与障碍物之间距离为σo,对于形状不规则的障碍物,σo表示无人艇距离障碍物质心的距离,障碍物质心坐标为

(8)

安全距离为d,则对第i优先级的障碍物有

(9)

对应的雅可比矩阵为

(10)

由上述条件可得,对于无人艇遇到的第i个障碍物的避障任务速度输出为

(11)

其中λ为避障任务的增益矩阵。避障任务的零空间向量为

(12)

最终避障任务速度输出

vo=v1+N1[v2+…+Ni-1vi]

(13)

3.2 质心任务

质心任务可以描述为欠驱动无人艇集群的质心到达目标点。在二维水面的情况下,质心定义如式(14)

(14)

根据先前的公式推导,可以得出

(15)

其中雅可比矩阵Jb∈2×2n为

(16)

根据式(4)质心任务的输出函数是

(17)

3.3 队形任务

由于零空间行为法存在编队稳定性不足的问题,因此本文采用虚拟结构法完成无人艇集群队形任务。将无人艇集群编队视为一个虚拟刚体,编队参考坐标系以集群质心为原点,正北方向为y轴正方向,正东方向为x轴正方向。参考坐标系运动则编队开始运动。因此队形任务可视为无人艇运动到质心的相对位置。任务函数定义为

(18)

其中pi是第i艘无人艇的坐标,pb=σb是无人艇集群质心的坐标。

简单起见,将向量写为

[η1,1,…ηn,1,η2,1,…ηn,2]T

(19)

雅可比矩阵Jb∈2×2n为

(20)

其中Jb∈2×2n为

(21)

σf表示无人艇集群的队形信息,一旦定义了队形σf就代表每艘无人艇在质心参考坐标系下的坐标。

在固定期望队形的情况下,队形任务的输出为

(22)

3.4 任务综合

在得到各个子任务的输出函数后,为避免低优先级任务与高优先级任务之间的冲突,根据子任务优先级顺序,将队形任务向量向质心任务零空间投影,得到队形任务与质心任务的综合输出,再将该向量投影到避障任务的零空间,得到最终的速度输出向量。任务综合后无人艇姿态示意图如图2。

图2 无人艇集群编队运动姿态示意图

根据式(6)以及上述子任务输出函数可以得到期望速度与期望角度如下

vx,y=vo+N1(vb+N2vf)

(23)

(24)

(25)

4 仿真试验

通过以上分析,本文使用MATLAB环境与基于QT Creat环境的无人艇集群控制系统软件作为实船仿真平台,验证本文所述方法的有效性,其中基于QT Creat环境的系统仿真软件模拟真实无人艇集群试验效果。

4.1 Matlab仿真结果

为测试无人艇集群编队功能,设置三艘无人艇组成无人艇集群,分别进行一字队形编队与三角队形编队,在仿真环境中设置三个半径不同的静态障碍物,此时可以认为对于集群中的单艘无人艇来说,环境内存在两个动态障碍物(即集群内其余两艘无人艇)。

无人艇集群初始化参数见表1,仿真环境信息见表2。

表1 无人艇参数初始化

表2 仿真环境信息

4.1.1 一字队形

无人艇集群一字队形仿真结果如图4所示,图5为无人艇集群角度图像。整个运动过程完成时间为102s,开始运动时,无人艇集群首先完成队形任务,到达指定编队位置后集群继续向目标点前进。第20s时无人艇1、2分别遇到障碍物,当障碍物中心与无人艇距离小于安全距离时,触发避障任务无人艇1、2开始自适应调整速度与角度完成避障在避障过程中,编队队形会被短暂破坏,但是能够保证无人艇编队安全航行。避障任务完成后,无人艇集群会迅速恢复编队队形。第48s时无人艇1遇到第二个障碍物,再次触发避障任务,无人艇1继续调整速度与角度。此时其余两艘无人艇虽然没有遇到障碍物,但是由于无人艇1避障队形发生改变导致集群质心偏离期望航线,其余两艘无人艇也会产生较小的角度变化,直至无人艇1避障完成,质心回归期望航线,集群恢复编队队形并且保持队形到达目标点。

图4 一字队形编队

图5 一字队形角度图像

4.1.2 三角队形

三角队形仿真结果如图6所示,角度变化图像如图7。由图6可以看出,欠驱动无人艇集群首先调整角度完成队形任务,第15s无人艇2最先遇到障碍物,开始避障任务,避障过程花费7s,无人艇1在第26s遇到障碍物,避障过程6s,避障成功后集群恢复编队。第57s无人艇1遇到第二个障碍物,避障时间为8s。第110s质心到达目标点,无人艇集群停止运动。

图6 三角队形编队结果

图7 三角队形角度图像

通过对以上MATLAB仿真结果分析可知,本文所述算法在理想状况下能够完成不同队形的编队任务,且队形的改变不会影响避障效果,保障了避障任务的最高优先级,集群能够在质心到达目标点时停止。

4.2 水面无人艇集群控制系统仿真结果

上述仿真验证了本文所述算法在理想条件下的有效性(即每艘无人艇可以在下一时刻调整为期望的速度与航向角,到达期望位置),但真实的欠驱动水面无人艇存在角度调整较慢,且为非线性加速,容易出现实际角度跟不上期望角度的问题。因此,为验证本算法在实船环境下的有效性,本文选择在完成MATLAB仿真后,使用基于QT Creat环境的欠驱动无人艇集群控制系统软件进行更为真实的仿真,仿真界面如图8所示,输入无人艇参数见表3。

表3 无人艇参数

图8 仿真系统显示界面部分截图

在欠驱动无人艇集群控制软件中,无人艇集群初始化参数见表4,仿真环境信息见表5。考虑到实际水面无人艇存在欠驱动性,当无人艇遇到障碍物航向角发生大角度变化时,无人艇航向角存在跟随延时,因此设置安全距离略大于障碍物半径,给予无人艇较大避障空间保证无人艇集群避障安全。

表4 无人艇仿真参数初始化

表5 仿真环境信息

图9、10为实船仿真中一字队形编队仿真路线图与无人艇2角度跟随误差图像。图11、12为三角队形编队仿真路线图和无人艇2角度跟随误差图像。

图9 一字队形仿真路线

图10 一字队形角度跟随图像

图11 三角队形仿真路线

图12 三角队形角度跟随图像

由图9、11可以看出,不同的编队队形对无人艇的避障路线选择会有影响但对避障任务的完成效果没有影响,无人艇集群在不同队形约束下都能够完成避障任务,质心任务以及队形任务。由图10、12分析可知,在遇到障碍物时,无人艇航向角变化虽然不能实时达到期望角度,但是跟随状况理想,能够保证在限制水域内,完成避障任务。

5 结语

本文基于零空间行为法,研究分析了欠驱动无人艇集群编队避障问题,零空间行为法可以有效避免无人艇集群处理复杂任务时各任务之间的冲突问题。针对无人艇编队避障问题,本文将该任务分解为三个优先级不同的子任务,并建立相应的任务输出函数,得到无人艇集群整体的任务输出模型。针对零空间行为法存在的无人艇集群编队稳定性差的问题,引入虚拟结构法,将无人艇集群质心作为编队坐标原点,集群内各艘无人艇只需与质心保持相对坐标即可完成编队。最后通过两种仿真,验证了算法的有效性与可行性,结果表明:对于不同队形要求的欠驱动无人艇集群,均可顺利完成集群编队,并在运动过程中避开环境中存在的障碍物,最终到达目标点。