一种基于栅格法的大规模UUV集群编队队形形成方法
技术领域
本发明属于UUV编队控制技术领域，具体涉及一种基于栅格法的大规模UUV集群编队队形形成方法。
背景技术
UUV在开发海洋资源，发展国民经济以及军事应用领域都有十分重要的意义。随着UUV技术的发展，越来越倾向于利用UUV集群协同作业以大幅提升作业能力和作业效率。UUV集群在执行协同探测、目标搜索等作业过程中，一般会采用以某一期望几何队形形态编队航行的方式。而编队航行首先要解决的问题就是如何实现UUV集群从初始随机分布、杂乱无序的几何队形形态，形成期望的几何队形形态，该过程就是UUV集群编队队形形成。
队形形成属于UUV编队控制领域，常用的编队控制方法如人工势场法、领航跟随法、虚拟结构法等在解决队形形成后的队形保持方面更加适用，但是应用到队形形成方面通常会存在通信信息交互量大、规划协调逻辑复杂、存在UUV间碰撞风险等问题。特别是UUV集群采用的是通信宽带小、通信延迟大的水声通信以及UUV集群的规模大时面临的难度更大。因此，非常有必要发展一种可应用于大规模UUV集群的编队队形形成方法。
申请号为201910917112.6的专利文件中公开了“一种基于圆周分层规划的UUV集群编队队形形成方法”。主要解决了针对通讯延迟有序机动的小规模UUV集群队形形成方法。首先该专利是采用扇形和圆周的空间区域划分方法，与本发明行区域与列区域相交的栅格空间区域划分方法不同。其次该专利重点提出了一种分步规划、分步机动，规划与机动交叉的小规模UUV集群的队形形成方法，与本发明重点解决了统一规划总体路径、统一机动的大规模UUV集群队形形成的方法不同。
发明内容
本发明的目的在于提供可应用于大规模UUV集群的编队队形形成的一种基于栅格法的大规模UUV集群编队队形形成方法。
本发明的目的通过如下技术方案来实现：包括以下步骤：
步骤1：从UUV集群中选择主UUV，其余UUV均为从UUV；
步骤2：设置行列区域宽度Lres和期望队形形成指令
xexp_E(n)为第n个从期望点在固定坐标系下的x坐标，yexp_E(n)为第n个从期望点在固定坐标系下的y坐标，n＝1,2,···,Nnum_UUV_F；Nnum_UUV_F为从UUV个数；
步骤3：队形形成开始，所有UUV保持定点、定艏向，并确定自己的主从身份；
步骤4：UUV集群进行信息交互，所有从UUV向主UUV发送自身当前位置信息
步骤5：以主UUV所在位置为原点建立船体直角坐标系，以间隔为Lres的直线将船体直角坐标系空间划分为由行区域和列区域叠加构成的空间栅格，并设定行区域和列区域的最大值以及最小值；
第i号从UUV在主UUV船体坐标系下的坐标Pf_pos_B(i)为：
第i号从UUV当前位置所在的行区域rowf(i)和列区域rankf(i)为：
步骤6：主UUV判断是否接收到所有从UUV位置信息；若主UUV在规定的时间内接收到所有从UUV当前位置信息，则执行步骤7；否则认为队形形成失败，主UUV向所有从UUV发送队形形成失败指令，所有UUV停车后返回步骤4；
步骤7：主UUV规划出各从UUV列分散、行分散、列机动、行机动四个阶段的机动目标点；
步骤7.1：主UUV求解出所有满足条件的从UUV行分散的行区域坐标、列分散的列区域坐标和所要到达的从期望点序号的矩阵X，并筛选出使所有从UUV总运动距离最短的矩阵X；
矩阵X＝(x(1),x(2),…,x(i))，
矩阵X满足的条件为：
1)当i≠j时，
2)当rowf(i)＝rowf(j)且i≠j时，(yM_PSO(i)-yM_PSO(j))(rankf(i)-rankf(j))＞0；
3)当yexp_E(PID_PSO(i))＝yexp_E(PID_PSO(j))时，
(xM_PSO(i)-xM_PSO(j))(yexp_E(PID_PSO(i))-yexp_E(PID_PSO(j)))＞0；
4)|xM_PSO(i)|＞1，|yM_PSO(i)|＞1；
5)当|yexp_M(PID_PSO(i))|≤1时，(xM_PSO(i))(xexp_M(PID_PSO(i)))＞0；
6)|xM_PSO(i)-<xM_PSO(i)>|+|yM_PSO(i)-<yM_PSO(i)>|+|PID_PSO(i)-<PID_PSO(i)>|＝0，其中< >表示取整运算；
7)当|rowf(i)|≤1时，(rankf(i))(yM_PSO(i))＞0；
步骤7.2：计算各从UUV列分散、行分散、列机动、行机动四个阶段的机动目标点的在固定坐标系下的坐标；
列分散点在固定坐标系下的坐标
行分散点在固定坐标系下的坐标和
列机动目标点在固定坐标系下的坐标
行机动目标点在固定坐标系下的坐标
步骤8：主UUV向所有从UUV发送各阶段的机动目标点信息，待所有从UUV反馈已接收到四个机动目标点的信息后，执行步骤9；
步骤9：主UUV向所有从UUV发送列分散指令，所有从UUV由当前位置向列分散目标点运动，每个从UUV完成列分散后向主UUV反馈信息；
步骤10：主UUV判断所有从UUV是否完成列分散；若主UUV在规定的时间内接收到所有从UUV完成列分散的反馈信息，则执行步骤11；否则认为队形形成失败，主UUV向所有从UUV发送队形形成失败指令，所有UUV停车后返回步骤4；
步骤11：主UUV向所有从UUV发送行分散指令，所有从UUV由当前位置向行分散目标点运动，每个从UUV完成行分散后向主UUV反馈信息；
步骤12：主UUV判断所有从UUV是否完成行分散；若主UUV在规定的时间内接收到所有从UUV完成行分散的反馈信息，则执行步骤13；否则认为队形形成失败，主UUV向所有从UUV发送队形形成失败指令，所有UUV停车后返回步骤4；
步骤13：主UUV向所有从UUV发送列机动指令，所有从UUV由当前位置向列机动目标点运动，每个从UUV完成列机动后向主UUV反馈信息；
步骤14：主UUV判断所有从UUV是否完成列机动；若主UUV在规定的时间内接收到所有从UUV完成列机动的反馈信息，则执行步骤15；否则认为队形形成失败，主UUV向所有从UUV发送队形形成失败指令，所有UUV停车后返回步骤4；
步骤15：主UUV向所有从UUV发送行机动指令，所有从UUV由当前位置向行机动目标点运动，每个从UUV完成行机动后向主UUV反馈信息；
步骤16：主UUV判断所有从UUV是否完成行机动；若主UUV在规定的时间内接收到所有从UUV完成行机动的反馈信息，则执行步骤17；否则认为队形形成失败，主UUV向所有从UUV发送队形形成失败指令，所有UUV停车后返回步骤4；
步骤17：主UUV向所有从UUV发送队形形成成功指令，所有UUV保持定点和定艏向，完成UUV集群编队队形。
本发明的有益效果在于：
本发明将UUV集群编队队形形成空间划分为栅格空间的行区域和列区域，可以有效的避免编队队形形成过程中UUV之间的碰撞。本发明使用粒子群优化算法对列分散、行分散、列机动和行机动四个机动过程的总路线进行统一规划，可以实现UUV集群编队队形形成过程中的有序机动协调，有利于实现大规模UUV集群的编队队形形成。本发明中，UUV集群之间的信息交互量少，计算简单，规划速度快，协调机动逻辑分明，易于工程实现。
附图说明
图1为UUV集群编队队形形成示意图。
图2为UUV集群编队期望队形及队形形成所在空间划分示意图。
图3为基于栅格法的大规模UUV集群编队队形形成方法流程图。
图4为UUV集群编队列分散原理示意图。
图5为UUV集群编队行分散原理示意图。
图6为UUV集群编队列机动原理示意图。
图7为UUV集群编队行机动原理示意图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明的目的是为了提供一种基于栅格法的、适用于大规模UUV集群的编队队形形成方法。本发明可使大规模UUV集群从初始随机分布、杂乱无序的形态，快速、安全的形成期望的队形。
一种基于栅格法的大规模UUV集群编队队形形成方法，包括以下步骤：
步骤一：初始化，设置UUV主从身份指令，设置队形形成参数，期望队形指令；
设置队形形成参数Nnum_UUV_F和Lres，其中Nnum_UUV_F为从UUV个数，Lres为行列区域宽度。
设置期望队形形成指令
步骤二：队形形成开始，所有UUV保持定点、定艏向，并确定自己的主从身份；
步骤三：UUV集群进行信息交互。所有从UUV向主UUV发送自身当前位置信息
步骤四：主UUV判断是否接收到所有从UUV位置信息。若主UUV判断在120秒内收到所有从UUV当前位置信息，转步骤五，否则认为队形形成失败，转步骤(二十一)；
步骤五：主UUV应用粒子群优化算法规划出各从UUV列分散、行分散、列机动、行机动四个阶段的机动目标点。主UUV根据粒子群优化算法对四个阶段进行统一规划，求解出总路径最短情况下，从UUV行分散的行区域坐标、列分散的列区域坐标和所要到达的从期望点序号的矩阵X＝(x(1),x(2),…,x(i))，
步骤五中，主UUV应用粒子群优化算法规划出各从UUV四个阶段的机动目标点具体包括：
步骤五(A)：求解各从UUV当前位置所在的行区域rowf(i)和列区域rankf(i)；
其中xf_pos_B(i)为从UUV在主UUV船体坐标系下x轴坐标，yf_pos_B(i)为从UUV在主UUV船体坐标系下y轴坐标。
步骤五(B)：应用粒子群算法求解矩阵X＝(x(1),x(2),…,x(i))；
求得的粒子群矩阵应满足以下条件：
(1)分散过程结束后所有从UUV在不同行区域和不同列区域，分配的从期望点不相同，有以下式子成立：
其中i≠j，i＝1,2,...,Nnum_UUV_F，j＝1,2,...,Nnum_UUV_F。
(2)为了避免分散过程从UUV之间的相互碰撞，同一行区域中的从UUV相对位置不发生改变，应满足如下条件：
若rowf(i)＝rowf(j)，i≠j，则有以下式子成立：
(yM_PSO(i)-yM_PSO(j))(rankf(i)-rankf(j))＞0。
(3)完成列机动过程后，可能导致同一列区域内同时存在多个从UUV，为了避免行机动过程中从UUV之间相互碰撞，行机动过程中，同一列区域中的从UUV相对位置不发生改变，应满足如下条件：
若yexp_E(PID_PSO(i))＝yexp_E(PID_PSO(j))，则有以下式子成立：
(xM_PSO(i)-xM_PSO(j))(yexp_E(PID_PSO(i))-yexp_E(PID_PSO(j)))＞0。
(4)为了各从UUV在机动的过程中不与主UUV发生碰撞，分散过程的行区域和列区域坐标绝对值大于1，应满足如下条件：
xM_PSO(i)|＞1，|yM_PSO(i)|＞1，i＝1,2,...,Nnum_UUV_F
(5)为了避免行机动过程中从UUV与主UUV发生碰撞，应满足如下条件：
若|yexp_M(PID_PSO(i))|≤1，则有以下式子成立：
(xM_PSO(i))(xexp_M(PID_PSO(i)))＞0，i＝1,2,...,Nnum_UUV_F。
(6)规划各从UUV的位置的值为整数，应满足如下条件：
xM_PSO(i)-<xM_PSO(i)>|+|yM_PSO(i)-<yM_PSO(i)>|+|PID_PSO(i)-<PID_PSO(i)>|＝0，i＝1,2,...,Nnum_UUV_F，其中< >表示对数进行取整运算。
(7)为了避免列分散过程中从UUV与主UUV发生碰撞，应满足如下条件：
若|rowf(i)|≤1，则有以下式子成立：
(rankf(i))(yM_PSO(i))＞0，i＝1,2,...,Nnum_UUV_F。
步骤五(C)：求解出各从UUV列分散目标点的行列区域坐标
列分散运动中，从UUV只改变所在的列区域，行区域坐标不变，求解出列分散目标点的行列区域坐标
行分散运动中，从UUV只改变所在的行区域，列区域坐标不变，求解出行分散目标点的行列区域坐标
计算出从UUV列分散点在固定坐标系下的坐标
计算出从UUV行分散点在固定坐标系下的坐标
步骤五(D)：由从期望点行列区域坐标
步骤六：主UUV向所有从UUV发送各阶段的机动目标点信息；
步骤七：主UUV判断所有从UUV是否接收到四个机动目标点信息。若主UUV判断在120秒内所有从UUV接收到列分散点坐标、行分散点坐标、列机动点坐标和行机动点坐标，转步骤八，否则认为有的从UUV没有接收到四个机动目标点信息，转步骤六；
步骤八：主UUV向所有从UUV发送列分散指令；
步骤九：从UUV进行列分散。所有从UUV由当前位置向列分散目标点运动，每个从UUV完成列分散后告知主UUV；
步骤十：主UUV判断所有从UUV是否完成列分散。若主UUV判断在120秒内所有从UUV完成列分散，转步骤十一，否则认为队形形成失败，转步骤(二十一)；
步骤十一：主UUV向所有从UUV发送行分散指令；
步骤十二：从UUV进行行分散。所有从UUV由当前位置向行分散目标点运动，每个从UUV完成行分散后告知主UUV；
步骤十三：主UUV判断所有从UUV是否完成行分散。若主UUV判断在120秒内所有从UUV完成行分散，转步骤十四，否则认为队形形成失败，转步骤(二十一)。
步骤十四：主UUV向所有从UUV发送列机动指令；
步骤十五：从UUV进行列机动。所有从UUV由当前位置向列机动目标点运动，每个从UUV完成列机动后告知主UUV；
步骤十六：主UUV判断所有从UUV是否完成列机动。若主UUV判断在120秒内所有从UUV完成列机动，转步骤十七，否则认为队形形成失败，转步骤(二十一)。
步骤十七：主UUV向所有从UUV发送行机动指令；
步骤十八：从UUV进行行机动。所有从UUV由当前位置向行机动目标点运动，每个从UUV完成行机动后告知主UUV；
步骤十九：主UUV判断所有从UUV是否完成行机动。若主UUV判断在120秒内所有从UUV完成行机动，转步骤二十，否则认为队形形成失败，转步骤(二十一)；
步骤二十：主UUV向所有从UUV发送队形形成成功指令，所有UUV保持定点、定艏向，转步骤二十二；
步骤二十一：主UUV向所有从UUV发送队形形成失败指令，所有UUV停车；
步骤二十二：UUV集群编队队形形成结束。
本发明的有益效果在于：
1.本发明将UUV集群编队队形形成空间划分为栅格空间的行区域和列区域，可以有效的避免编队队形形成过程中UUV之间的碰撞。
2.本发明使用粒子群优化算法对列分散、行分散、列机动和行机动四个机动过程的总路线进行统一规划，可以实现UUV集群编队队形形成过程中的有序机动协调，有利于实现大规模UUV集群的编队队形形成。
3.本发明中，UUV集群之间的信息交互量少，计算简单，规划速度快，协调机动逻辑分明，易于工程实现。
结合图1，介绍UUV集群编队队形形成的过程。
如图1所示，以包含5个成员的UUV集群进行介绍。初始状态，集群UUV的几何形态是随机分布且杂乱无序的，每个UUV处于待机状态，等待编队队形形成开始指令。期望形成的几何队形为图中所示的矩形队形。当队形形成开始后，所有的UUV根据自己事先被赋予的身份信息，确定自己是主UUV还是从UUV，如图中0号UUV为主UUV，其余为从UUV。主UUV保持定位、定艏向，UUV集群之间经过信息交互、自主规划、协调机动后，在保证不发生相互碰撞的前提下形成期望的矩形队形。队形形成过程中，从UUV进行机动，主UUV始终保持定点、定艏向。
结合图2，介绍UUV集群编队队形形成所在空间划分及期望队形。
如图2，以矩形为期望的几何队形、以包含5个成员的UUV集群为例介绍期望队形。期望点0、1、2、3、4组成期望的矩形队形。在UUV集群形成矩形队形后，每个UUV位于一个期望点上，其中主UUV位于期望点0上，期望点0称为主期望点。其他4个从UUV分布于期望点1、2、3、4上，称这4个期望点为从期望点。
为了更好的利用栅格法和粒子群优化算法进行大规模UUV集群队形形成规划，将UUV集群所在空间区域划分为多个行区域和列区域，具体的划分过程为：
(1)行区域。如图2所示，以主UUV所在位置为原点建立的船体直角坐标系中，用相邻之间间隔为Lres的直线x＝k·Lres，将x轴方向的空间划分成不同的行区域，并且规定在x轴正方向上的行区域坐标为正值，负方向上的行区域坐标为负值，并有：
Nl_min-Nnum_UUV_F≤k≤Nl_max+Nnum_UUV_F
(2)列区域。同样在划分列区域时，是将y轴方向的空间划分成不同的列区域，利用相邻之间间隔为Lres的直线y＝m·Lres将空间分成多个列区域，并规定在y轴正方向上的行区域坐标为正值，负方向上的行区域坐标为负值，并有：
Nr_min-Nnum_UUV_F≤m≤Nr_max+Nnum_UUV_F
行区域和列区域叠加共同构成UUV所在水平面的空间栅格模型。其中k和m均为整数，Nl_min和Nl_max为所有从UUV和从期望点在主UUV船体直角坐标系下的所在位置的行区域的最小值和最大值；Nr_min和Nr_max为所有从UUV和从期望点在主UUV船体直角坐标系下的所在位置的列区域的最小值和最大值。
为了更清楚的介绍本发明的具体实施方式有如下定义：
(1)定义在基于栅格法建立的空间模型中，空间中任意一点固定坐标系下坐标为
其中
(2)定义在基于栅格法建立的空间模型中，根据空间中任意一点在主UUV船体坐标系下的坐标
其中xM为点所在行区域坐标，yM为点所在列区域坐标。若xM＝0，则规定xM＝1；若yM＝0，则规定yM＝1。
根据上面的定义，结合图2，则若已知空间中1号从UUV的固定坐标系下的坐标为
其他从UUV同理。
(3)若已知空间中任意一点的行列区域坐标
若已知1号从UUV在空间模型中的行列区域坐标
其他从UUV同理。
同理，运用(1)、(2)、(3)和(4)公式可计算从期望点在固定坐标系下的坐标
结合图3、图4、图5、图6和图7，介绍基于栅格法的大规模UUV集群编队队形形成方法的流程。
步骤一：初始化。设置UUV主从身份指令，设置队形形成参数Nnum_UUV_F和Lres，其中Lres为每个行区域和列区域的宽度，设置期望队形形成指令
步骤二：队形形成开始，所有UUV保持定点、定艏向，并确定自己的主从身份。
步骤三：UUV集群进行信息交互。所有从UUV向主UUV发送自身当前位置信息
步骤四：主UUV判断是否接收到所有从UUV位置信息。若主UUV判断在120秒内收到所有从UUV当前位置信息，转步骤五，否则认为队形形成失败，转步骤(二十一)。
步骤五：主UUV应用粒子群优化算法规划出各从UUV列分散、行分散、列机动、行机动四个阶段的机动目标点。主UUV根据粒子群优化算法对四个阶段进行统一规划，求解出总路径最短情况下，从UUV行分散的行区域坐标、列分散的列区域坐标和所要到达的从期望点序号的矩阵X＝(x(1),x(2),…,x(i))，进而计算出各从UUV四个阶段的机动目标点在固定坐标系下的坐标，列分散目标点坐标
其中i＝1,2,...,Nnum_UUV_F，
步骤五(A)：求解各从UUV当前位置所在的行区域rowf(i)和列区域rankf(i)。根据主UUV和从UUV的当前位置，利用公式(1)得到各从UUV当前位置点在主UUV船体坐标系下的坐标为
其中xf_pos_B(i)为从UUV在主UUV船体坐标系下x轴坐标，yf_pos_B(i)为从UUV在主UUV船体坐标系下y轴坐标。
步骤五(B)：应用粒子群算法求解矩阵X＝(x(1),x(2),…,x(i))，并满足如下要求：
(1)分散过程结束后所有从UUV在不同行区域和不同列区域，分配的从期望点不相同，有以下式子成立：
其中i≠j，i＝1,2,...,Nnum_UUV_F，j＝1,2,...,Nnum_UUV_F。
(2)为了避免分散过程从UUV之间的相互碰撞，同一行区域中的从UUV相对位置不发生改变，应满足如下条件：
若rowf(i)＝rowf(j)，i≠j，则有以下式子成立：
(yM_PSO(i)-yM_PSO(j))(rankf(i)-rankf(j))＞0。
(3)完成列机动过程后，导致同一列区域内可能同时存在多个从UUV，为了避免行机动过程中从UUV之间相互碰撞，行机动过程中，同一列区域中的从UUV相对位置不发生改变，应满足如下条件：
若yexp_E(PID_PSO(i))＝yexp_E(PID_PSO(j))，则有以下式子成立：
(xM_PSO(i)-xM_PSO(j))(yexp_E(PID_PSO(i))-yexp_E(PID_PSO(j)))＞0。
(4)为了各从UUV在机动的过程中不与主UUV发生碰撞，分散过程的行区域和列区域坐标绝对值大于1，应满足如下条件：
xM_PSO(i)|＞1，|yM_PSO(i)|＞1，i＝1,2,...,Nnum_UUV_F
(5)为了避免行机动过程中从UUV与主UUV发生碰撞，应满足如下条件：
若|yexp_M(PID_PSO(i))|≤1，则有以下式子成立：
(xM_PSO(i))(xexp_M(PID_PSO(i)))＞0，i＝1,2,...,Nnum_UUV_F。
(6)规划各从UUV的位置的值为整数，应满足如下条件：
xM_PSO(i)-<xM_PSO(i)>|+|yM_PSO(i)-<yM_PSO(i)>|+|PID_PSO(i)-＜PID_PSO(i)>|＝0，i＝1,2,...,Nnum_UUV_F，其中< >表示对数进行取整运算。
(7)为了避免列分散过程中从UUV与主UUV发生碰撞，应满足如下条件：
若|rowf(i)|≤1，则有以下式子成立：
(rankf(i))(yM_PSO(i))＞0，i＝1,2,...,Nnum_UUV_F。
步骤五(C)：求解出各从UUV列分散目标点的行列区域坐标
由于列分散运动中，从UUV只改变所在的列区域，所以行区域坐标不变，行分散运动中，从UUV只改变所在的行区域，所以列区域坐标不变。由粒子群优化算法得到的矩阵，根据公式(5)得出列分散目标点的行列区域坐标
根据公式(6)计算出行分散目标点的行列区域坐标
将
步骤五(D)：由从期望点行列区域坐标
由于是先列机动再行机动，所以可由从期望点行列区域坐标
再根据公式(8)得到行机动目标点在固定坐标系下的坐标
步骤六：主UUV向所有从UUV发送各阶段的机动目标点信息。
步骤七：主UUV判断所有从UUV是否接收到四个机动目标点信息。若主UUV判断在120秒内所有从UUV接收到列分散点坐标、行分散点坐标、列机动点坐标和行机动点坐标，转步骤八，否则认为有的从UUV没有接收到四个机动目标点信息，转步骤六。
步骤八：主UUV向所有从UUV发送列分散指令。
步骤九：从UUV进行列分散。所有从UUV由当前位置向列分散目标点运动，每个从UUV完成列分散后告知主UUV。
如图4所示，4号从UUV与3号从UUV在相同的列区域，所以4号从UUV需要由当前位置点(虚线处)运动到4号从UUV的列分散目标点(实线处)，使3号和4号两从UUV在不同列区域，其他从UUV保持原有位置不变。
步骤十：主UUV判断所有从UUV是否完成列分散。若主UUV判断在120秒内所有从UUV完成列分散，转步骤十一，否则认为队形形成失败，转步骤(二十一)。
步骤十一：主UUV向所有从UUV发送行分散指令。
步骤十二：从UUV进行行分散。所有从UUV由当前位置向行分散目标点运动，每个从UUV完成行分散后告知主UUV。
如图5所示，1号从UUV与2号从UUV在相同的行区域，所以1号从UUV需要由当前位置点(虚线处)运动到1号从UUV的行分散目标点(实线处)，其他从UUV保持原有位置不变。
步骤十三：主UUV判断所有从UUV是否完成行分散。若主UUV判断在120秒内所有从UUV完成行分散，转步骤十四，否则认为队形形成失败，转步骤(二十一)。
步骤十四：主UUV向所有从UUV发送列机动指令。
步骤十五：从UUV进行列机动。所有从UUV由当前位置向列机动目标点运动，每个从UUV完成列机动后告知主UUV。
如图6所示，1号从UUV需要由当前位置点(虚线处)向1号从UUV的列机动目标点(实线处)运动，其他从UUV同理。
步骤十六：主UUV判断所有从UUV是否完成列机动。若主UUV判断在120秒内所有从UUV完成列机动，转步骤十七，否则认为队形形成失败，转步骤(二十一)。
步骤十七：主UUV向所有从UUV发送行机动指令。
步骤十八：从UUV进行行机动。所有从UUV由当前位置向行机动目标点运动，每个从UUV完成行机动后告知主UUV。
如图7所示，1号从UUV需要由当前位置点(虚线处)向1号从UUV的行机动目标点(实线处)运动，其他从UUV同理。
步骤十九：主UUV判断所有从UUV是否完成行机动。若主UUV判断在120秒内所有从UUV完成行机动，转步骤二十，否则认为队形形成失败，转步骤(二十一)。
步骤二十：主UUV向所有从UUV发送队形形成成功指令，所有UUV保持定点、定艏向，转步骤二十二。
步骤二十一：主UUV向所有从UUV发送队形形成失败指令，所有UUV停车。
步骤二十二：UUV集群编队队形形成结束。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
