基于动态目标点协同的无人艇编队路径规划

郝金玉,范 雪,杨少龙,赵三飞,卢忆文

(1.驻大连地区第一军事代表室， 辽宁 大连 116000； 2.华中科技大学船舶与海洋工程学院， 武汉 430074； 3.中国舰船研究设计中心， 武汉 430064)

1 引言

随着各国对海洋开发重视程度日益提高，基于水面无人艇(unmanned surface vehicles,USV)的海洋自主作业成为探索开发海洋资源的重要利器。随着智能化程度提升，无人艇可实现自主航行、环境感知、目标探测、自主避障等功能。相比有人海上作业装备，无人艇具有体积小、速度快、机动性强等优势，可完成高风险、重复性、大范围的海上作业任务，在海上侦察、海事搜救、海洋调查等场景有良好应用前景。随着应用需求日益广泛，无人艇编队理念也应时而生。相比于单艇作业，编队作业在覆盖范围、容错率、实效性等方面具有更好表现，在大范围海洋作业方面具有重要意义。

无人艇编队如何进行路径规划，提高编队的队形稳定性和安全性，一直是该领域的研究热点。目前，国内外已有较多专家学者针对该问题展开研究并取得进展。欧阳子路等针对无人艇编队稳定性问题，在经典快速搜索随机树算法基础上，提出了一种非严格保形修正向量与非严格保形控制圆区域，能够使无人艇编队安全避碰并最大程度地保证队形的稳定性。李贺等为解决复杂扰动下的无人艇编队控制问题，设计一种基于有限时间扰动观测器的固定时间编队控制策略，有效提高无人艇编队系统的精确性和鲁棒性。谢伟杰等考虑到基于质点模型的蜂拥算法直接应用到欠驱动无人艇模型的困难性，提出一种基于目标跟踪的转化策略，能够有效地将蜂拥算法间接地应用到无人艇集群编队中。

在编队控制领域，相比于以上相关论文中所用算法，领航者-跟随者控制算法是实现多机器人编队最经典、最简单的控制算法。核心思想是基于领航者的实时位置，按一定的编队位置关系得出跟随者的实时位置，进而实现整个机器人集群的协同编队。该算法的缺点就是过度依赖领航者。在确定每一个跟随者的位置时只考虑了与领航者的位置关系，没有考虑跟随者之间的位置关系，有可能造成跟随者机器人碰撞等问题出现。

人工势场算法是另一多机器人编队控制领域的经典算法。人工势场算法的优点在于可以实现机器人集群运动中不发生碰撞，且势场中力的计算是根据机器人之间的实时相对位置，时效性强。相比于领航者-跟随者算法，编队效果更加灵活。人工势场的缺点主要有两点：第一，存在“零势能点或区域”，机器人集群在势场力的作用下，向零势能区域运动，当机器人到达此区域后，不再受势场力的作用，此时机器人将无法继续运动；第二，人工势场算法只保证机器人集群之间不发生碰撞，它具有改变编队队形的能力而没有维持队形的能力，这与我们期望能够实现多机器人的固定编队运动有偏差。

在前人研究基础上，结合以上两种算法的优缺点，提出将领航者-跟随者和人工势场两种算法融合的动态目标点编队路径规划策略，同时考虑到将用于质点模型算法应用到无人艇编队中的困难性，提出目标追踪的转化策略，进一步构建无人艇分层控制架构，设计通用视线角制导律(line of sight,LOS)与经典比例-积分-微分(proportion integration differentiation,PID)控制律的航路跟踪算法，将多无人艇编队任务分解成领航船的目标航线跟踪和跟随艇的动态目标点跟踪。根据领航船的实时位置，更新动态目标点的位置，并保证跟随艇的持续追踪。仿真结果表明，该策略可较好地实现多无人艇的直线和圆形等基本航路单元的编队任务，保持刚性编队稳定性同时使碰撞冲突对编队稳定性的影响降至最低。

2 协同编队路径规划设计

无人艇协同编队路径规划策略以系统中各个无人艇的行为和相互作用研究为出发点，为突出算法的准确性和降低工作复杂性，用图论描述多无人艇编队系统。如图1所示，用节点1、2、3、4描述4艘无人艇，用双箭头连线描述两两对象的通信信息流。为模拟真实自组网通信拓扑方式(如ZigBee、Wifi组网)的准确性，设定任意2个节点间都可实现信息交互，即多无人艇系统内任何一个个体都可收到其他个体发送的信息流。

图1 节点模型描述无人艇编队系统示意图Fig.1 Node model representation of USV formation

根据图论中图的结构和性质可用矩阵描述，定义矩阵=[]代表系统中节点联系，当两节点,之间有信息流时，=1；定义对角矩阵=diag；定义图拉普拉斯矩阵=-。

结合图1实例，则有：

(1)

式(1)中3个矩阵是协同编队策略的关键变量。

2.1 基于领航者-跟随者的编队群体运动算法

节点模型如式(2)：

(2)

式中:、分别代表节点的当前状态量、输入控制量；∈，代表系统中的节点个数，这里取4。输入控制量由式(3)得到：

(3)

式中:为编队邻接矩阵；为节点的相邻集合。将式(3)代入式(2)，同时引入全局状态向量=[,…,]∈，整理得式(4)。

(4)

式(4)可见编队稳定性由矩阵决定。由于矩阵每一行元素相加为零，所以当且仅当有唯一的特征向量，保证=0，其特征值为零，该结论对实现节点编队控制有重要意义。式(5)可实现编队稳定性，最终状态为：

(5)

式中:为特征向量；为可逆矩阵；(0)为节点在0时刻的状态量。图1中，假设节点4为领航者，节点1、2、3为跟随者。编队中跟随者的控制函数为：

(6)

式中:>0为常数;为时刻；为跟随者节点和跟随者节点之间的相对位置。编队中领航者控制算法为

(7)

式中:()为时刻领航者节点与跟随者节点之间的相对距离；和为定值；()为在时刻领航者与期望航点之间的距离。根据式(6)和式(7)，可实现节点集群按照一定固定编队队形运动。

2.2 基于人工势场的编队个体间避障算法

在领航者-跟随者编队运动算法基础上，融入人工势场法，保证在形成和维持编队队形过程中节点之间不发生碰撞。假设在时刻，节点扫描到了节点处于危险接触范围内，此时节点会对节点产生一个斥力满足

(8)

式中:为节点个数，()=||-()||，为探测距离，为一常数，在时刻避碰响应()对节点速度产生影响。

则考虑编队个体间斥力影响下，编队中跟随者节点的控制函数更新为：

(9)

式中:为常数。编队中领航者节点的控制算法更新为

(10)

引入避碰斥力分量，式(9)和式(10)协同能够实现单个节点在集群的形成和维持编队队形过程中避免碰撞。

3 无人艇编队分层控制架构

3.1 分层控制架构设计

为实现面向多无人艇的协同编队自主航行任务，在协同编队路径规划层输出动态目标点，引入无人艇单艇控制层，如图2所示，通过分层控制思想将无人艇编队自主航行任务解耦成基于协同编队路径规划策略的动态质点编队控制的规划层和基于LOS-PID底层控制算法的无人艇动态目标点跟踪的控制层。

图2 无人艇编队控制层设计框图Fig.2 Hierarchical control design framework of USV formation

控制原理如图3所示，在某时刻下，协同编队控制策略中的式(9)依据领航者的实时位置(,)和动态目标点和在上一时刻末的位置，产生动态目标点和在该单位时间内的运动速度，目标点的初始位置和运动速度累加，实现不断更新目标点的位置。动态目标点与领航者连线构成的阵型即为无人艇编队的期望队形，将动态目标点和作为无人艇和的跟踪目标，最终达到多无人艇协同编队的效果。

图3 分层控制原理示意图Fig.3 Schematic diagram of layered control

3.2 LOS-PID底层控制算法

LOS制导律是无人艇航路跟踪控制的经典理论，具有逻辑简洁、参数易调、稳定性好等优点。LOS制导原理如图4所示，只与无人艇的当前位置和任务目标点有关，能够将二维航路跟踪问题转化为无人艇的一维航向跟踪问题，实现无人艇逐步平滑逼近目标航线及跟踪目标点。

图4 LOS制导原理示意图Fig.4 LOS guidance principle diagram

在惯性坐标系下由一系列的目标点(=1,2,3,…)组成期望航线，无人艇的路径跟踪任务按顺序依次到达各航路点并沿着相邻2个航点之间的连线跟踪，当无人艇到达某航点附近时执行到点切换逻辑，实现顺次跟踪多航路点目标。

以航点和 + 1 之间的航线任务为例，无人艇当前位置为()，以无人艇为圆心设定半径为的视线圆，以视线圆与+1的交点()为期望位置。根据反正弦函数可求出()()和()()之间的夹角。当无人艇与航线间的距离大于时，直接取无人艇到航线的垂线方向作为期望航向。根据该逻辑，则期望航向可表示为式(11)：

(11)

式中:的选取直接影响无人船对目标航线的跟踪响应速度，当越小则期望航向越趋近于垂直航线方向，系统对目标航线的横向偏差收敛响应越快，这种情况下无人艇位置会快速逼近目标航线上，但同时也会带来振荡和不稳定现象。当较大时则系统对横向误差的响应较慢，但控制过程会相对稳定。

当根据动态目标点生成无人艇期望航向后，底层控制器根据期望航向产生舵角控制指令，控制无人艇航向逼近期望航向。设计PID航向控制器，由于结构简单、调试方便、工程实现度高，广泛应用于实船航向控制。

以LOS制导产生的期望航向与无人艇当前航向之间的偏差信号为PID的输入，根据航向偏差分别进行比例、积分、微分运算，为防止长时间积分使积分项过大导致执行器指令饱和，对于积分运算进行了饱和限制，即设定一个最大值，当积分运算结果达到这个值后，不再进行累加。如式(12)所示，PID控制律将比例、积分、微分三项运算结果相加得到舵角控制指令。

(12)

式中:()为舵角控制指令；()为航向偏差；为离散采样点，=0,1,2,…；为比例系数；为积分系数，一般取0；为微分系数。

4 Matlab仿真验证

根据第3节所设计的动态目标点协同编队路径规划策略、LOS-PID制导控制底层算法，设计无人艇编队执行仿真系统，流程如图5所示。

图5 无人艇编队控制仿真流程框图Fig.5 USV formation control simulation process framework

按照执行任务的先后逻辑，首先为领航船单艇的路径跟踪任务，如图5中箭头、、所示，根据目标航线和领航船的当前位置，根据LOS-PID制导控制算法输出舵角控制指令，同时设定无人船转速保持恒定且不为0，根据舵角和转速更新领航船的实时位置。然后，协同编队控制算法根据所设计的期望队形和领航船的实时位置计算出3个动态目标点的位置，如图5中箭头、所示。最后，3个跟随艇实时跟踪并不断缩短与各自目标点的差距，如图5中箭头、、所示，最终形成无人艇编队的期望队形。本章的Matlab仿真验证设计也将按如图5所示流程执行(本文采用Matlab R2020a进行仿真验证)。

4.1 控制层验证

首先进行单艇路径跟踪仿真试验，初始以(0,0)为起始点，设计了由9个航点组成的期望航线，如图6，该航线包含了基本的直线与转向路径，可较好地确定参数值。

图6 期望航线Fig.6 Desired routes

设定LOS制导律中视线圆半径为4 m，到点判断距离为1 m。PID控制器、、三个参数进行Matlab仿真，依据单一变量原则，依次确定3个参数值。

首先，防止长时间积分下不能收敛为一有限值，设置饱和限制，为简化参数取值，将积分系数设为0。其次，比例系数对系统控制影响最大，系数越大，动态反应越快，消除的能力越强，但由于惯性作用，系数过大时会产生振荡不稳定，在参数选取时首先确定；最后，微分系数可以优化反应速度，避免过大带来的不稳定效应，主要起辅助稳定的作用，参数取值中最后确定。

从无人艇的追踪轨迹、产生期望航向和当前航向的响应以及无人艇相对于期望轨迹的实时位置偏差3个方面，对控制算法进行验证。

经过Matlab数值仿真确定=6,=0，且发现当=6,=0,5,10时，的数值变化对反应速度的优化效果不明显，对系统减小超调，克服振荡，加快系统的响应速度，减小调节时间，改善系统的动态性能的作用较低，为减少参数个数，取=0。

经Matlab仿真，如图7所示，使用LOS制导律和PID控制器能够完成单艇的路径跟踪任务，LOS制导律产生期望航向，当期望航向与当前航向产生差值时，在PID控制的作用下，对无人艇模型产生舵角控制信号，实现无人艇沿照期望航线运动。此外，从图8可以看出，无人艇的期望航向和当前航向仅在转弯处产生一定超调和震荡，系统可以及时做出反应并迅速消除差值，能够实现无人艇的及时转向跟踪。如图9所示，无人艇模型在跟踪图6轨迹时，仅在转弯处有明显的横向偏差，但最大值不超过2 m，可说明单艇的跟踪航线能力较好。

图7 仿真无人艇追踪轨迹Fig.7 USVfollowing trajectory

图8 期望航向和当前航向曲线Fig.8 Curve of desired course and current course

图9 横向位置偏差曲线Fig.9 lateral position deviationcurve

4.2 规划层验证

单艇的航路跟踪控制算法验证完成后，验证协同编队控制算法的有效性，即根据领航船与跟随船的质点模型能否实现固定编队运动，并且运动过程中质点之间不会发生碰撞。

初始位置设定：领航船(0,0)、目标点1(-1.5,1.5)、目标点2(1.5,1.5)、目标点3(-3,0)。在经过一段时间稳定后，领航船位置为(,)时，3个动态目标点分别在轴正向、轴正向、轴负向上相差20，期望队形可由领航船与动态目标点的位置共同构成三角形编队。式(9)和式(10)的参数如表1所示。

表1 算法主要参数Table 1 Main parameters of the algorithm

首先测试直线编队，领航船以1 m/s速度沿轴直线运动。结果如图10(a)所示。然后，测试圆轨迹编队，领航船以1 m/s速度沿半径为10 m的圆做圆周运动。结果如图10(b)所示。

图10(a)展示了在直线编队任务中领航船和3个目标点的运动轨迹，实现了在协同编队控制算法下实现某一初始位置的质点模型编队，当一段时间后编队处于稳定状态，按照期望的相对位置协同前进。图10(b)展示了曲线运动任务，领航船和3个目标点的运动轨迹和相对位置，曲线编队稳定性不如直线，但基本符合预期位置。因此，规划层中协同编队控制算法对质点模型编队的控制效果符合期望预期。

(虚线和○代表领航船和3个目标点的运动轨迹和相对位置；实线和△代表无人艇编队的运动轨迹和3个跟随者的相对位置)图10 多无人艇协同编队仿真曲线Fig.10 Simulation verification of multi-USVs formation

4.3 规划层与控制层联合验证

在单独验证规划层和控制层基础上，将2部分级联验证分层控制架构及编队任务的可行性，3艘跟随艇与目标点初始位置一致，初始航速为1 m/s，初始航向沿轴正向为0°。同规划层验证类似，验证直线和曲线航行编队。结果如图10(c)和10(d)所示。图10(c)展示了3艘无人艇跟踪动态目标点行进轨迹和多无人艇编队的构型，从行进轨迹上可见无人艇编队较好地追踪动态目标点的运动轨迹，实现了避碰和编队效果。

图10(d)所示为曲线行进轨迹，稳定后，3个跟随艇相对于领航船分别在轴正向、轴正向、轴负向上相差20 m，较好地保持无人艇的期望队形。

经过3个阶段验证，所提算法对多无人艇编队运动控制是理论可行的，既能保证无人艇编队的固定队型，也能实现在行进过程中不发生碰撞。

对仿真结果进行分析，将其与使用人工势场算法控制无人艇柔性编队运动的策略进行对比，如图11所示，该算法通过引入一个虚拟领航者，使其沿轴直线运动，采用分层控制策略，规划出一系列目标点引领整个集群运动，该算法可以实现无人艇集群沿轴正向运动，并且无人艇之间不会发生碰撞，但该算法无法实现无人艇集群形成并保持固定的编队队形，无人艇之间位置没有明确相对关系。所提协同编队控制算法相比于该人工势场算法，如图10(c)和(d)所示，在能够实现无人艇之间避碰的基础上，同时实现无人艇集群按照期望队形进行编队，3个跟随者相对于领航者在轴正向、轴正向、轴负向上分别相差20 m，无人艇之间有明确的相对位置关系，更易于无人艇编队协同作业。故所提算法相比于该人工势场算法，从编队形成和维持编队的稳定性上都体现出来更好的性能。

图11 人工势场柔性编队无人艇运动仿真曲线Fig.11 The artificial potential field algorithm controls the swarm motion of USVs

5 结论

提出了融合领航者-跟随者协同编队与人工势场避碰的多无人艇编队路径规划策略。设计分层控制架构，在规划层实现质点模型编队输出3个跟随艇的动态目标点，控制层利用LOS-PID算法实现无人艇目标点跟踪。所提方法将编队任务解耦成动态目标点的规划与跟踪2个子问题，较好地实现了稳定且安全的协同编队任务。经Matlab仿真验证，所提算法及系统能够对直线、圆弧两类基本路径单元实现较好的协同编队，为进一步开展复杂路径协同编队研究提供了良好支撑。

协同编队算法和分层控制的复杂度低、稳定性好、通用性强，不仅可用于多无人艇编队任务，也可拓展至异构集群，为实际海上应用提供了一种简洁有效的协同编队策略。