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

485第4 0 卷第6 期2023年6 月真机仿算文章编号：10 0 6-9 34 8(2 0 2 3)0 6-0 4 8 5-0 6欠驱动无人艇集群协同编队路径规划仿真王艺凯，孙俊，王（江南大学人工智能与计算机学院，江苏无锡2 14 12 2）摘要：针对欠驱动无人艇集群协同编队路径规划问题，在零空间行为法的基础上引入虚拟结构法，解决了零空间行为法存在的编队稳定性不足的问题。通过将欠驱动无人艇集群编队任务分解成三个优先级不同的子任务，分别求解每个子任务的任务输出函数，最后将所有子任务的任务输出函数进行综合，得到综合的任务输出函数。在MATLAB环境与基于QT Creat环境的无人艇仿真控制平台进行了仿真验证，仿真结果证明，上述算法的有效性与可行性，欠驱动无人艇集群能够达到编队路径规划目的。关键词：无人艇；零空间行为法；虚拟结构法；仿真控制平台；编队路径规划中图分类号：TP391.9文献标识码：BSimulation of Path Planning for Under-driven UnmannedSurface Vehicle Cluster Cooperative FormationWANG Yi-kai,SUN Jun,WANG Si(School of Artificial Intelligence and Computer,Jiangnan University,Wuxi Jiangsu 214122,China)ABSTRACT:In response to the path planning problem of under-driven unmanned surface vehicle(USV)cluster col-laborative formation,the virtual structure method is introduced on the basis of the null-space behavior method to solvethe problem of insufficient formation stability in the null-space behavior method.By decomposing the under-drivenUSV cluster formation task into three subtasks with different priorities,the output function of each subtask was solvedseparately,and finally the task output functions of all subtasks were synthesized to obtain a comprehensive task outputfunction.The simulation verification was carried out in the MATLAB environment and the USV simulation controlplatform based on the QT Creat environment.The simulation results proved the effectiveness and feasibility of the al-gorithm.The under-driven USV cluster can achieve the purpose of formation path planning.KEYWORDS:USV;Null-space behavior method;Virtual structure method;Simulation control platform;Formationpath planning1引言无人艇（Unmanned SurfaceVehicle,USV）是一种具备自主运行能力的水面艇，作为新一代信息技术的产物，无人艇不仅是军事无人作战系统的重要组成部分，而且为提升国家科技创新能力提供了重要支撑1。由于海洋环境日益复杂、作业任务日益多样，无人艇系统处理复杂任务往往需要相互间的协作才能更有效率地、更高质量地完成。在海军作战系统中，一艘无人艇可以实现有限的功能，而多艘USV协同编基金项目：国家自然科学基金（U1836218）；国家自然科学基金(61672263)。收稿日期：2 0 2 1-10-2 0修回日期：2 0 2 1-10-2 9队不仅可以提高效率，还可以完成许多更复杂的任务，如协同防护、编队集结等，因此无人艇集群化必然是无人艇发展的重要方向2 3。编队控制问题作为无人艇集群化研究领域的重要课题，引起了国内外众多学者的关注。目前国内外对多智能体集群编队控制问题的研究方法主要有领航者-跟随者(Leader-Follower,L-F)法4、基于行为(Behaviour-Based)法5】、虚拟结构（VirtualStructure）法6)】、人工势场（ArtificialPotential Field,APF)法7 领航跟随法的主要实现形式是编队中设置一艘无人艇作为领航者，其余无人艇作为跟随者，跟随者与领航者保持固定的相对位置形成队形8。Sun等9 采用基于有限控制集模型预测控制方法提高了跟随者的自治性。Park等10 提出一种基于连通性保持和避障性能函数的鲁棒主从编队跟486踪设计。领航跟随法的优点是队形稳定性高，容易设计实现，缺点是过度依赖领导者，跟随者反馈差，避障效果差。虚拟结构法要求机器人以刚体上的相应点的位置和姿态信息作为各自的跟踪目标形成队形。Tank等12 提出了虚结构法,该方法能取得较高的精度和轨迹跟踪效。Ren13采用信息一致性算法，提出了一种分布式虚拟结构的多智能体编队算法。虚拟结构法的优点是队形保持性能良好、系统稳定性容易保证，缺点是队形不灵活、避障不易实现。人工势场法是通过设置势场函数，无人艇在势场的吸引力、排斥力作用下,得到相应的队形。Sang等14 提出在人工势场模型中引入A*算法，生成全局最优路径，降低无人艇陷人局部极小值的概率。人工势场法的缺点在于，队形的变化需要对势场函数的重新配置，因而不够灵活。基于行为控制方法则是将任务整体分解成简单的子任务，系统内的智能体根据自身的性能被分配到不同的子任务，就需要有不同的行为来完成子任务。Tucker和Ronald15最早采用自下而上构造系统的方式，提出了一种基于行为的控制方法，将队形控制任务分解为一系列的基本行为，通过行为的综合来实现运动控制。基于行为法的优点则是可解决多任务问题、避障效果好，缺点是队形控制的稳定性得不到保证。本文采用零空间行为法，对欠驱动无人艇集群编队路径规划问题进行研究。零空间行为法的核心是将低级任务向量向高级任务的零空间进行投影，将综合的任务输出作为最终的任务输出，在完成高级任务的同时部分或全部完成低级任务，具有实时性强、任务之间不冲突的优点16 本文在国内外研究的基础上，对整个问题进行任务分解，在任务分解完成后对不同子任务完成任务函数的处理，将虚拟结构法与零空间行为法相结合，保证了无人艇集群编队的稳定性，通过仿真验证所述算法在无人艇集群编队路径规划问题上的可行性。2零空间行为法建模行为法的主要思想是首先设定一些智能体的期望行为，每个行为都有其对应的目标或任务。在欠驱动无人艇集群编队路径规划任务中，零空间行为法首先将任务分解为多个可以独立完成的子任务并分别计算出各个子行为的任务函数。为解决各子任务之间可能存在的任务冲突，首先对各子任务划分优先级，其次将低优先级任务的任务向量向高优先级任务的零空间投影，从而抵消低优先级任务对高优先级任务的任务冲突，最终得到任务综合的输出函数，传输给底层的无人艇运动控制器控制无人艇的运动。设无人艇状态向量r=x,其中和是无人艇在水面坐标系下的坐标位置，是无人艇航向角，U=u,是无人艇线速度,其中是无人艇横向速度分量，U,是无人艇纵向速度分量。定义为要控制的任务变量，则其函数模型可表示为g=f(r)(1)对应的微分关系为=af(r)/ar=J(r)u(2)-无人艇31无人艇2;质心111无人艇1；103图1无人艇集群运动模型其中J(r）是r的输出雅可比矩阵。在无人艇运动过程中通过求取最小范数速度，从而得到最小二乘解Ua=Jtod(3)其中伪逆矩阵J=J（J）-,为任务期望。在任务的完成过程中，无人艇的运动轨迹可以通过ud的时间积分来获得。为消除离散时间积分导致的无人艇坐标位置偏移，可将式（3）改写为如下形式Ua=Jt(oa+Ao)(4)式（4）表示单个任务的速度输出，其中是增益矩阵，表示任务误差,定义为=Qa-Q。由式（4)可知，在多任务的情况下，各任务速度计算公式如下V;=J t(oi.d+A,o,)(5)其中下标i表示第i个子任务。如果下标也表示任务的优先级（即任务1是最高优先级的任务），在n个子任务的情况下,则式(5)可以被改写为Ud=Ui+N,u2+.+Nn-Iu,(6)式（6)的几何解释为每个任务速度的计算是单独进行的，在计算该速度对总的速度贡献之前，用1表示适当维数的单位矩阵，将第i优先级的任务通过式（7）投影到上一优先级任务的零空间上，以抵消该任务对高优先级任务的影响N,(J)=I-JIJ,(7)一方面，基于零空间的行为控制是在非奇异配置下完成最高优先级的任务。另一方面，较低优先级的任务仅在子空间中完成，在子空间中，它们不与具有较高优先级的任务冲突，也就是说，每个任务达到次优条件，该次优条件根据由最487其中oi(9)2D一P高优先级任务施加的约束来优化任务。3住任务处理在欠驱动无人水面艇集群编队任务中，子任务包括以下三项：避障任务、质心任务、队形任务。下面分别对这三项子任务进行任务处理3.1避障任务避障任务是优先级最高的任务，对于保证无人艇集群的完整性至关重要。欠驱动无人艇集群运动过程中可能遇到两种障碍物第一种为环境中存在的礁石一类集群外障碍物，第二种为集群中除自身外的其它无人艇，因此欠驱动无人艇集群的避障任务不是一项集群任务，而是每艘无人艇的单独任务。在前进方向存在障碍物时，无人艇避障的目的就是使无人艇与障碍物保持安全距离。因此，避障任务将无人艇障碍物方向上的速度作为输出，该速度保证无人艇与障碍物之间的距离不小于安全距离。当无人艇遇到多个障碍物时，将无人艇与障碍物之间距离作为避障优先级排序依据，距离最近的障碍物优先级最高，分别计算无人艇对每个障碍物的避障速度。设无人艇与障碍物之间距离为。，对于形状不规则的障碍物，。表示无人艇距离障碍物质心的距离，障碍物质心坐标为P。=x i3(8)安全距离为d,则对第i优先级的障碍物有对应的雅可比矩阵为J:=FT(10)其中，P-PoiT=是无人艇到障碍物质心方向的单位向量。Tp-Poil/由上述条件可得，对于无人艇遇到的第i个障碍物的避障任务速度输出为U;=Jt入(d-go.)(11)入为避障任务的增益矩阵。避障任务的零空间向量为N,=I-FFT(12)最终避障任务速度输出V。=U +N,u 2 +.+N;-1U,(13)3.2质心任务质心任务可以描述为欠驱动无人艇集群的质心到达目标点。在二维水面的情况下，质心定义如式（14)10,=fi(p1,.,)=Pi(14)ni=1其中p=xy:是第i艘无人艇的坐标。根据先前的公式推导，可以得出af.(p)U;=J.(p)u(15)i=1ap;3.4任务综合其中雅可比矩阵J.ER2x2n为1O(16)01根据式（4）质心任务的输出函数是v,=Jt(ob.d+A,o,)(17)其中，为质心期望位置与当前位置的误差。3.3队形任务由于零空间行为法存在编队稳定性不足的问题，因此本文采用虚拟结构法完成无人艇集群队形任务。将无人艇集群编队视为一个虚拟刚体，编队参考坐标系以集群质心为原点，正北方向为轴正方向，正东方向为轴正方向。参考坐标系运动则编队开始运动。因此队形任务可视为无人艇运动到质心的相对位置。任务函数定义为Pi-Pi(18)LPn-Pb其中p：是第i艘无人艇的坐标,P,=Q。是无人艇集群质心的坐标。简单起见，将向量写为(19)雅可比矩阵J,eR2x2n为厂AJ=(20)LOA其中 J,eR2x2n为111nn111nA(21)二1111，表示无人艇集群的队形信息，一旦定义了队形，就代表每艘无人艇在质心参考坐标系下的坐标。在固定期望队形的情况下，队形任务的输出为U,=J/4,o,(22)在得到各个子任务的输出函数后，为避免低优先级任务与高优先级任务之间的冲突，根据子任务优先级顺序，将队形任务向量向质心任务零空间投影，得到队形任务与质心任务的综合输出，再将该向量投影到避障任务的零空间，得到最终的速度输出向量。任务综合后无人艇姿态示意图如图2。根据式（6)以及上述子任务输出函数可以得到期望速度与期望角度如下=D+N,(U+N2u)(23)(24)+488净目标点队形期望点无人艇2障碍物无人艇3质心无人艇11102图2无人艇集群编队运动姿态示意图arctan(25)14仿真试验通过以上分析，本文使用MATLAB环境与基于QTCreat环境的无人艇集群控制系统软件作为实船仿真平台，验证本文所述方法的有效性,其中基于QTCreat环境的系统仿真软件模拟真实无人艇集群试验效果。4.1Matlab仿真结果为测试无人艇集群编队功能，设置三艘无人艇组成无人艇集群，分别进行一字队形编队与三角队形编队，在仿真环境中设置三个半径不同的静态障碍物，此时可以认为对于集群中的单艘无人艇来说，环境内存在两个动态障碍物（即集群内其余两艘无人艇）。无人艇集群初始化参数见表1，仿真环境信息见表2。4.1.1一字队形表1无人艇参数初始化无人艇1无人艇2无人艇3坐标1(5,0,0)(-5,-5,0)(-10,1,0)线速度v00一字编队(10,0)(0,0)(-10,0)三角编队(10,0)(10,10)(0,10)表2 优仿真环境信息目标点障碍物安全距离/m最大速度m/s(20,10,3)(70,70)(40,60,2)52(10,3,1)无人艇集群一字队形仿真结果如图4 所示，图5为无人艇集群角度图像。整个运动过程完成时间为10 2 s，开始运动时，无人艇集群首先完成队形任务，到达指定编队位置后集群继续向目标点前进。第2 0 s时无人艇1、2 分别遇到障碍物，当障碍物中心与无人艇距离小于安全距离时，触发避障任务无人艇1、2 开始自适应调整速度与角度完成避障在避障过程中，编队队形会被短暂破坏，但是能够保证无人艇编队安全航行。避障任务完成后，无人艇集群会迅速恢复编队队形。第4 8 s时无人艇1遇到第二个障碍物，再次触发避障任务，无人艇1继续调整速度与角度。此时其余两艘无人艇虽然没有遇到障碍物，但是由于无人艇1避障队形发生改变导致集群质心偏离期望航线，其余两艘无人艇也会产生较小的角度变化，直至无人艇1避障完成，质心回归期望航线，集群恢复编队队形并且保持队形到达目标点。无人艇集群编队仿真90BarycenterAgentsObstacles80无人艇3无人艇270无人艇160403020100-100102030406060708090轴/m图44一字队形编队无人艇角度图像75901001011512选代次数/次无人#10 152026303840 4650588068707810010511011512选代次数/次400无人习/选代次数/次图5一字队形角度图像4.1.2三角队形三角队形仿真结果如图6 所示，角度变化图像如图7。由图6 可以看出，欠驱动无人艇集群首先调整角度完成队形任务，第15s无人艇2 最先遇到障碍物，开始避障任务，避障过程花费7 s，无人艇1在第2 6 s遇到障碍物，避障过程6 s，避障成功后集群恢复编队。第57 s无人艇1遇到第二个障碍物，避障时间为8 s。第110 s质心到达目标点，无人艇集群停止运动。通过对以上MATLAB仿真结果分析可知，本文所述算法在理想状况下能够完成不同队形的编队任务，且队形的改变不会影响避障效果，保障了避障任务的最高优先级，集群能够在质心到达目标点时停止。489无人艇集群编队仿真90Barycenter80Agentsobstacles无人艇370无人艇260无人艇15040302010F-10-20-100102030405060708090 x轴/m图6三角队形编队结果无人艇角度图像40038032020024020016051200510202530364046505580657075808590951001051110115120选代次数/次400360320280买2 4 0宣200180120510152025303540455055607075859095100105110115120选代次数/次400A3/0505560659510010511D11512D选代次数/次图7三角队形角度图像4.2水面无人艇集群控制系统仿真结果上述仿真验证了本文所述算法在理想条件下的有效性（即每艘无人艇可以在下一时刻调整为期望的速度与航向角，到达期望位置），但真实的欠驱动水面无人艇存在角度调整较慢，且为非线性加速，容易出现实际角度跟不上期望角度的问题。因此，为验证本算法在实船环境下的有效性，本文选择在完成MATLAB仿真后，使用基于QTCreat环境的欠驱动无人艇集群控制系统软件进行更为真实的仿真，仿真界面如图8 所示，输人无人艇参数见表3。图：仿真系统显示界面部分截图在欠驱动无人艇集群控制软件中，无人艇集群初始化参数见表4，仿真环境信息见表5。考虑到实际水面无人艇存在欠驱动性，当无人艇遇到障碍物航向角发生大角度变化时，无人艇航向角存在跟随延时，因此设置安全距离略大于障碍物半径，给予无人艇较大避障空间保证无人艇集群避障安全。表3天无人艇参数型长/m型宽/m平均吃水/T横稳系数纵稳系数推进方式40.71.71.51.8单浆表4无人艇仿真参数初始化无人艇1无人艇2无人艇3坐标(26,0,0)(-27,-3,0)(0,-1,0)线速度v000一字编队(20,0)(-20,0)(0,0)三角编队(15,0)(-15,0)(0,15)表5仿真环境信息目标点障碍物安全距离/m最大速度m/s(38,3,3)(50,100)(10,13,3)72(17,60,5)图9、10 为实船仿真中一字队形编队仿真路线图与无人艇2 角度跟随误差图像。图11、12 为三角队形编队仿真路线图和无人艇2 角度跟随误差图像。图9 一字队形仿真路线由图9、11可以看出，不同的编队队形对无人艇的避障路线选择会有影响但对避障任务的完成效果没有影响，无人艇集群在不同队形约束下都能够完成避障任务，质心任务以及队形任务。由图10、12 分析可知，在遇到障碍物时，无人艇航向角变化虽然不能实时达到期望角度，但是跟随状况理想，能够保证在限制水域内，完成避障任务。49080期望航向角实际航向角0402025507100125150时间/s图10一字队形角度跟随图像图11三角队形仿真路线期望航向角50一实际航向角4030201002655075100125150时间/s图12三角队形角度跟随图像5丝结语本文基于零空间行为法，研究分析了欠驱动无人艇集群编队避障问题，零空间行为法可以有效避免无人艇集群处理复杂任务时各任务之间的冲突问题。针对无人艇编队避障问题，本文将该任务分解为三个优先级不同的子任务，并建立相应的任务输出函数，得到无人艇集群整体的任务输出模型。针对零空间行为法存在的无人艇集群编队稳定性差的问题，引人虚拟结构法，将无人艇集群质心作为编队坐标原点，集群内各艘无人艇只需与质心保持相对坐标即可完成编队。最后通过两种仿真，验证了算法的有效性与可行性，结果表明：对于不同队形要求的欠驱动无人艇集群，均可顺利完成集群编队，并在运动过程中避开环境中存在的障碍物，最终到达目标点。参考文献：1谢少荣，刘坚坚，张丹.复杂海况无人艇集群控制技术研究现状与发展J.水下无人系统学报，2 0 2 0,2 8（6)：13.2李家良.水面无人艇发展与应用J.火力与指挥控制，2 0 12，(6):205-209.3谢伟，陶浩，龚俊斌，等.海上无人系统集群发展现状及关键技术研究进展J.中国舰船研究，2 0 2 1,16(1）：7-17,31.4李娟，袁锐锟，张宏瀚.基于领航跟随法的多AUV编队控制算法研究J.仪器仪表学报，2 0 19,4 0（6）：2 4 0-2 4 9.5徐澎.基于行为的多AUV队形控制技术研究D.上海交通大学,2 0 13.6李正平，鲜斌.基于虚拟结构法的分布式多无人机鲁棒编队控制J.控制理论与应用，2 0 2 0,37（11)：154-16 2.7易国，毛建旭，王耀南，等.非完整移动机器人领航-跟随编队分布式控制J.仪器仪表学报，2 0 17，（0 9）：2 2 6 6-2 2 7 2.8Sun X,Wang G,Y Fan,et al.A Formation Collision AvoidanceSystem for Unmanned Surface Vehicles with Leader-FollowerStructureJ.IEEEAccess,2019:1-1.9Park B S,Yoo S J.Connectivity-maintaining and collision-avoiding performance function approach for robust leaderfollowerformation control of multiple uncertain underactuated surfacevesselsJ.Automatica,2021,127(9):109501.10潘无为，姜大鹏，庞永杰，等.人-工势场和虚拟结构相结合的多水下机器人编队控制J.兵工学报，2 0 17，38（2）：32 6-334.11Tan K H,Lewis M A.Virtual structures for high-precision cooper-ative mobile robotic controlC.1996 IEEE/RSJ InternationalConference,1996:132-139.12 Wei R.Decentralization of Virtual Structures in Formation Controlof Multiple Vehicle Systems via Consensus Strategies J.European Journal of Control,2008,14(2):93-103.13Sang H,You Y,Sun X,et al.The hybrid path planning algorithmbased on improved A*and artificial potential field for unmannedsurface vehicle formationsJ.Ocean Engineering,2021,223(3-4):108709.14Ba Lch T,Arkin R C.Behavior-based formation control for multi-robot teams J.IEEE Transactions on Robotics&Automation,1998,14(6):926-939.15庞师坤，梁晓锋，李英辉，等。基于零空间行为法的自主水下机器人避障策略J.上海交通大学学报，2 0 2 0，54（3）：7 9-88.作者简介王艺凯（19 9 6-），男（汉族），黑龙江哈尔滨人，硕士研究生，主要研究领域为多智能体集群算法。孙俊（19 7 1-），男（汉族），江苏无锡人，博士、教授，主要研究方向为人工智能、计算智能、机器学习、集群控制算法等。王（19 9 6-），女（汉族），江苏苏州人，博士研究生，主要研究方向为多智能体系统算法优化。