一种自主水下航行器编队航路规划算法*

余耀宇 蔡 超

（华中科技大学人工智能与自动化学院多谱信息处理技术国家级重点实验室 武汉 430074）

1 引言

自主水下航行器（Autonomous Underwater Vehicle，AUV）已经成为人类探索和开发海洋的重要工具［1］。随着任务复杂性的增加，单个AUV在任务执行过程中的局限性越来越明显，此时需要通过多个AUV编队协作来完成任务，从而突破单航行器在感知，决策及执行能力等方面的限制。目前实现AUV编队航行常用的控制方法有领航跟随法［2］、虚拟结构法［3］、基于行为法［4］和人工势场法［5］等。但是水下环境复杂且通信困难，实现编队控制是一件非常困难的事情。提前规划好编队航路，可以降低AUV编队控制的复杂度，减少AUV编队间通信的信息量。编队航路规划通常要求航路满足编队队形要求，或者确保航行器同时到达指定区域从而形成编队。国内外许多学者针对形成编队的多航路规划做了大量研究［6～9］，大多没有考虑编队保持阶段的航路规划。对于编队保持阶段的航路规划，文献［10］基于A*算法将无人机编队看作一个整体，以编队中心作为规划结点，得到一组具有编队约束的三维航迹；文献［11］通过对编队航行任务的分解，采用粒子群算法用来解决编队航路规划问题，生成了具有队形约束的协同航路，但是没有考虑编队转弯的情况，导致编队在转弯前后的状态不一致。

粒子群优化算法（Particle Swarm Optimization）［12］是一种全局优化进化算法，可以很好地用于求解多航路规划问题。粒子群算法解的好坏和收敛速度受初始化粒子质量的影响，本文引入通视性分析和混沌映射的思想来改善初始化粒子的质量，既保证了初始化粒子的有效性又保证了随机性，将该方法应用于编队航路规划，通过分析编队航行的过程，针对不同的航行阶段规划出不同的协同航路，解决粒子群算法在进行编队航路规划时存在的编队转弯问题，实现编队保持阶段的队形与状态一致性。

2 编队航行问题及约束条件

2.1 编队航行问题

编队航行任务通常由编队形成、编队保持、编队执行任务这三个主要的阶段组成。在进行编队规划之前，需要确定AUV编队的任务，AUV数目以及编队的队形。编队形成阶段是指AUV由完全无序的状态按照某种规则在规定区域形成编队的过程，在编队形成阶段，往往需要为AUV规划出一组从起始点到达队形约束形成点的协同航路。在编队保持阶段，需要AUV编队在航行过程中保持稳定的队形，通常是采用非线性控制器来实现，但是由于水下环境复杂，水声通信微弱，动力学的非线性和强耦合性，这对于控制系统的实时性和鲁棒性提出了较高的要求。因此可以考虑在编队保持过程中规划出具有队形约束的航路，从而减轻控制系统和通讯系统的负担。编队执行的任务是由任务规划系统决定，本文的编队任务为同时打击多个目标，因此同样也需要为AUV规划出一组满足时间与空间约束的协同航路。

图1 编队航行示意图

通过分析编队航行的过程，按照是否需要维持编队队形，可以将编队航行过程划分为编队保持阶段和编队非保持阶段。在编队保持阶段，编队中的AUV之间要保证相应的队形关系，同时确保状态的一致性；在编队非保持阶段（编队形成阶段，编队执行任务阶段），需要满足时间上的同时到达和空间上的避免碰撞。

在编队保持阶段规划航路时，需要考虑编队转弯问题，确保编队转弯前后的队形和状态的一致性。状态一致性指的是AUV编队的队形应与AUV航行方向始终维持一个固定的角度关系；队形一致性指的是编队中AUV应始终维持相应的位置关系。如图2（a）航路，编队在转弯前后保证了队形是一字型，但没有保证状态一致性；图2（b）航路，通过设置编队转弯航路，保证队形和状态的一致性。

图2 编队转弯前后的队形和状态一致性

2.2 编队航行问题

实验中所涉及的环境区域主要有限制区、风浪区、威胁区、陆地和岛屿。不同类型的区域有不同的约束限制。限制区是限制航行的区域，也即规划的航路不能通过限制区。风浪区则是指海平面上的波浪区域，禁止AUV在风浪区进行上浮，因此上浮点不能位于风浪区内。威胁区一般为敌方军舰，这些区域对于航行器来说具有一定威胁性，有一定概率被摧毁，可将威胁代价定义为

其中l表示AUV穿过威胁区的路径长度，ε为威胁系数。

时间协同约束是指在发射多个AUV后，AUV能够按照预定的时间到达各自的目标位置；空间协同约束是指在任意时间节点上，AUV之间的距离应大于最小安全距离。时间协同约束和空间协同约束可如式（2）所示：

其中，上标T表示目标点，i和j为AUV的编号，k为AUV的数目。为第i个AUV的到达目标点的时间，Dt(xi,yi)表示第i个AUV在t时刻的空间位置坐标。Tmax为允许航路时间代价差值的最长时间间歇，Dmin为最小安全距离。

编队保持阶段需要编队间保持相应的队形关系，本文以领航跟随法为队形约束基准，根据领航者的位置，确定出其余跟随者的位置。队形约束可表示为

其中Pl(t)表示领航者AUV在t时刻的空间位置，Pi(t)表示第i个跟随者AUV在t时刻的空间位置。D(x,y)和A(x,y)分别为计算x与y之间距离和夹角的函数，Li和θi分别表示第i个跟随者AUV与领航者AUV的期望距离与夹角。

在编队保持过程中，不可避免遇到转弯，编队转弯会导致AUV编队各自的航路长度不一致，这时候需要控制算法控制AUV的航行速度，外侧的速度要大于内侧速度，才能在转弯过程中保持编队队形。以一字型编队为例，假设AUV内侧的拐弯半径为R，编队转弯前的航行方向与正北方向的夹角为θi，转弯后的航行方向与正北方向的夹角为θi+1，AUV间的距离为d。转弯过程中两段航路的距离差为：。以内侧AUV为基准，编队转弯的时间代价为

3 基于粒子群算法的编队航路分段规划

结合编队航行的特性，可以将编队航路规划分为编队保持阶段的规划和编队非保持阶段的规划两个步骤。

步骤一：确定队形约束形成点和队形约束结束点的位置，并得到中间为了避障的编队转弯开始点和结束点，以及编队转弯圆弧。步骤二：得到起始点到队形约束形成点，以及队形约束结束点到目标点的满足时间协同约束和空间协同约束的航路。

步骤一得到的导航点称为一级导航点，步骤二得到的导航点称为二级导航点。

3.1 编队保持阶段的航路规划

采用粒子群算法解决单航路规划问题时，往往将起始点和目标点以及之间的导航点作为一个粒子。受虚拟结构法的启发，将编队中所有的航路看作一个粒子，假设一共有K个AUV，N个一级导航点，粒子的形式如式（5）所示：

上式中一级导航点均需满足式（3）对应的队形约束条件。但是由于未考虑编队转弯的情况，此时编队保持阶段的航路虽然满足了队形一致性，但并未满足编队的状态一致性，这样采用粒子群算法得到的编队航路示意图如图2（a）所示。对于每一个需要转弯的导航点，以内侧航路为基准，按照最小编队转弯半径，得到内侧航路的编队转弯开始点和编队转弯结束点和转弯圆弧。并根据AUV编队的位置关系，得到其他航路的编队转弯开始点和编队转弯结束点以及转弯圆弧。

1）通视-混沌粒子初始化

粒子群算法的寻优能力和收敛速度受初始化粒子影响，通常希望初始化粒子的分布具有很好的均匀性，增强粒子的多样性。对于编队航路规划来说，复杂的海洋环境以及编队航路约束等条件使得算法较难找到最优解，增加粒子种群的规模，搜索的范围也越大，越容易找到最优解，但是会极大增加算法的复杂度。如果初始化的粒子已经是较优粒子，则会加快算法的收敛速度，但是因为缺乏粒子的多样性，粒子群算法很可能会陷入局部最优解。

如何平衡粒子群算法的寻优能力和收敛速度是一大难题。对于实际问题来说，在对航路进行离线预规划时往往侧重于寻优能力；但是对于在线航路规划问题，更侧重于收敛速度，不必苛求所求解是否最优。假设初始化时粒子种群大小为L，较优的粒子个数为Lbetter，分布均匀的粒子个数为Lrandom，满足：

μ取值0＜μ＜1，μ较大时粒子的收敛速度越快，μ较小时全局搜索能力强。通视分析［13］本质上是判断观测点与目标点之间的视线是否通达，如果遇到障碍物则绕过，到达下一个观测点，最终到达目标点，非常适用于约束区域较为稀松的广阔海洋规划环境的快速规划。本文采用通视分析来构建较优的初始粒子，如图3所示，E、T分别为起始点和目标点，首先判断两点之间是否通达，发现其穿过限制区1，得到绕过限制区的路径EAT和EBT，选择较短路径EBT，将当前任务分解为E到B和B到T的两个子任务，判断两个子任务是否通达，对于不通达的路径进一步分解，以此类推，直到所有连线不穿过任何禁行区域为止，选出其中最短的路径，图中ECBT即为所求。

图3 通视分析原理示意图

以某一航行器为例，采用通视分析得到该AUV从起始点到达目标点的航路，将航路按照一定间隔均匀分为N个一级导航点，包括一个队形约束开始点、一个队形约束结束点以及N-2个中间导航点。按照队形约束关系，得到其余AUV的航路，这样便得到了一个较优的初始化粒子（本文称为通视粒子）。然后分别以其余AUV构建通视粒子，最终得到K个通视粒子。

Logistic混沌具有较好的均匀分布性［14］，其基本公式为

Xi表示第i个混沌向量，θ为预先设定常数。当θ=4时，系统在［0，1］内处于混沌状态。假设粒子群算法中粒子的维数为d，随机生成一个d维向量X0，向量的每个维度取值为0～1之间。根据式（7）可以映射得到l个混沌向量，将每个混沌向量的每个维度分量映射到粒子对应维度的取值区间上，即可得到l个分布均匀粒子（本文称为混沌粒子）。

在编队航路规划的初始化上，以其中一个AUV为基准，采用混沌映射方式得到l个混沌向量，按照队形约束条件，得到其他的AUV航路，这样便得到了l个混沌粒子。然后分别以其余AUV为基准进行相应的混沌初始化，从而可以得到l×K个混沌粒子。

2）粒子适应度

如图4中导航点的定义，假设有K个AUV，N个一级导航点，N-2个转弯点，则对应会有N-2个转弯开始点和转弯结束点，需要优化的目标为

图4 编队规划航路示意图

式中，f(x,y)表示从x点到达y点的时间代价。xy可能是直线或圆弧。如果未经过任何环境区域直线的时间代价为两点距离除以AUV速度；圆弧的时间代价如式（4）所示。如果xy经过了禁行区域（陆地、岛屿或限制区），则时间代价为无穷大；如果经过了威胁区域则除了xy本身的时间代价还需要加上式（1）的威胁代价。FA为编队起始点到队形约束形成点和队形约束结束点到目标点的时间代价，采用通视分析进行预估。在K个AUV到达队形约束形成点的预估时间代价中，取最大值作为该阶段的基准协同时间RefTime；对于队形约束结束点到达目标点的预估时间代价也采用同样的方式得到基准协同时间。FB+FC为编队保持阶段的时间代价，FB为编队保持阶段直线环节的时间代价，FC为编队转弯部分的时间代价，对于编队转弯部分，在计算粒子适应度时需要得到编队转弯圆弧的代价，但是在粒子进行迭代寻优的过程中，保持转弯点的不变。此外，为使编队保持阶段的航路尽可能长，FA需乘以一个大于1的加权系数ε。

3）粒子位置与速度更新策略

粒子群算法是通过跟踪个体极值和群体极值，从而更新粒子的速度和位置，其更新策略为

式中，wmax和 wmin，c1max和 c1min，c2max和 c2min分别代表惯性权重w，学习因子c1和c2的最大最小值，t为当前迭代次数，Tmax为最大迭代次数。

最后，将粒子中的需要转弯的导航点按照最小转弯半径为基准，转换为转弯开始点和转弯结束点以及编队转弯圆弧，从而得到编队保持阶段的航路。

3.2 编队非保持阶段的航路规划

在矢量规划空间上，不同起始点和目标点规划出来的航路产生空间碰撞的概率很小，无需在进行航路规划的过程中实时进行空间碰撞检测，只需在得到多组协同航路后进行空间碰撞检测从而筛选出最优航路即可。粒子群算法可以一次性产生多条航路，可以将这多条航路按照空间位置进行分类，也即将粒子种群划分为多个粒子子群，从这些子群中各自筛出其中的最优航路，这样即可得到多条空间散布均匀的航路。本文采用K均值聚类算法对粒子群算法产生的多条航路进行分类。

1）混沌粒子初始化

采用粒子群算法进行协同航路规划时，需要建立多个粒子种群来实现，其中每一个粒子种群对应一个AUV。相较于编队保持阶段的航路规划，这一环节算法复杂度相对较小，无需设置通视粒子，同样采用混沌映射的方式，对种群进行初始化。

2）粒子适应度

在协同航路规划中，时间协同约束条件与单航路规划时的时间代价是冲突的，最优的单航路规划的集合是很难满足协同任务需求的，而且编队非保持阶段的航路要确保AUV几乎同时到达目标点。因此编队非保持阶段的规划代价除了威胁代价以外还需要考虑时间协同代价，用航路的时间代价与RefTime的差值来描述。对于某个AUV的航路，其第i个粒子的优化目标为

其中，yi表示第i个粒子对应的航路，ftime(y)表示航路时间代价，fthreat(y)表示威胁代价。α和β为权重因子，根据实际情况来设定其大小。

4 编队航路规划实验分析

为验证编队航路规划算法的有效性和可行性，针对不同的环境约束条件以及编队任务设计多组实验。实验所用的规划地图为S57矢量电子海图，除岛屿外，还添加了限制区，威胁区，风浪区。AUV的速度为5nmi/h（约9.26km/h），编队最小转弯半径R为2.2km。粒子群算法的参数最大最小值为c1max=2.3，c1min=0.1，c2max=1.8，c2min=0.1，wmax=1.2，wmin=0.6。现分别针对三角形编队，菱形编队，一字型编队设计三组实验。三角形编队的队形为由三个AUV构成的底边为1.1km的等腰直角三角形；菱形编队由四个AUV构成，边长为0.777km；一字型编队由六个AUV构成，每两个临近的AUV间的距离为0.55km。实验结果中，红色旗子是起始点，蓝色三角是目标点，黑点是队形约束形成与结束点，黄点、白点分别为编队转弯开始、结束点，棕色点为上浮开始和结束点，绿色点为二级导航点。

针对复杂环境下三角形编队实验，其起始点的经 纬 度 为（126.494°，26.907°），（126.388°，26.897°），（126.307°，26.871°）；目标点的经纬度为（127.016°，26.251°），（127.000°，26.203°），（126.935°，26.153°）。规划出的航路如图5所示，AUV编队航行的关键结点时间代价如表1所示。

图5 复杂环境下三角形编队规划航路

表1 复杂环境下三角形编队时间代价

针对复杂环境下菱形编队的实验，其起始点的经纬度为（126.544°，26.907°），（126.438°，26.897°），（126.357°，26.871°），（126.283°，26.849°）；目标点的经纬度为（126.656°，26.084°），（126.732°，26.110°），（126.788°，26.139°），（126.842°，26.171°）。规划出的航路如图6所示，AUV编队航行的关键结点时间代价如表2所示。

图6 复杂环境下菱形编队规划航路

表2 复杂环境下菱形编队时间代价

针对一字型编队的规划实验，其起始点的经纬度为（122.250°，29.310°），（122.245°，29.290°），（122.239°，29.270°），（122.233°，29.250°），（122.239°，29.230°），（122.244°，29.210°）；结束点的 经 纬 度 为（123.114°，29.430°），（123.120°，29.405°），（123.126°，29.380°），（123.132°，29.355°） ，（123.137°，29.335°），（123.132°，29.300°）。规划出的航路如图7所示，AUV编队航行的关键结点时间代价如表3所示。

表3 一字型编队时间代价

图7 一字型编队规划航路

从以上三组不同编队在不同环境下的编队规划的实验结果可知，所提算法能够规划出一组满意的编队航路。在编队保持阶段可以很好地保持队形结构，针对不同的复杂环境也能够以较优的方式转弯绕行，保证了编队队形和状态的一致性，航路间的时间代价基本一致，且编队保持阶段的航路长度在整个编队航行过程中占比较高；在编队非保持阶段，各AUV间的航路满足时间和空间协同的要求。

5 结语

编队航路规划可划分为编队保持阶段的规划和编队非保持阶段的规划。在编队保持阶段，采用粒子群算法，将编队航路看作一个粒子，对粒子进行通视-混沌初始化，通过迭代寻优得到一组具有队形约束的航路，且保证了编队转弯前后队形和状态的一致性；在编队非保持阶段，采用粒子群算法，以K均值聚类的方式得到一组满足时间要求且空间分布散列的协同航路。实验结果表明，该算法得到的编队航路满足设定目标要求。