CN112650214B - 一种集群***动态编队的队形控制方法 - Google Patents

Abstract

本发明公开了一种集群***动态编队的队形控制方法包括：基于环境威胁地图为领航主机进行最优路径搜索，综合考虑最短路径与最小障碍威胁获得综合最优路径；将上述最优路径进行参数化将上述参数化后的路径信息再优化；通过编队信息为其余跟随从机分配跟随路径并参数化；根据通信条件，通过速度控制量为编队路径完成队形协调控制；对编队过程中后加入的障碍完成避障控制；周期更新路径信息，并将最新的路径参数化矩阵在集群***中更新同步。本发明解决了现有集群***编队控制方法难以在通信条件有限情况下，完成复杂曲线编队精度不高的问题。

Description

一种集群***动态编队的队形控制方法

技术领域

本发明涉及集群智能体***编队控制技术领域，特别涉及一种集群***动态编队的队形控制方法。

背景技术

集群***采用集群的方式可以拓展单体***的能力，高效合作协调完成任务，各单体之间互相功能备份，可以避免单体失效造成任务失败。集群***一般都需要通过保持一定的编队队形来完成任务，且编队队形的精度可能会直接影响任务的完成与否。目前集群编队可分为静态与动态两种。静态编队通过在地面站根据编队造型与变换预先为各单体规划好航迹，并为各航迹点设置统一的时间戳，将规划的航迹存储在各单体的内部存储设备中，在编队任务开始时由地面站广播发送启动命令，后续每个集群单体可以按照时间戳同步跟踪预设路径，可以获得很高的编队精度，且运动过程无需通信，抗干扰能力强，但是在编队过程中无法更改编队信息，只能用于编队表演或者固定路径巡检等。

对于动态编队，目前集群编队路径大部分算法研究中，均采用从起点到终点的最短路径搜索，一般只考虑避开障碍的空间位置，缺乏障碍的类型判断与预处理。因此寻找一种合适的方法对障碍信息进行更合理的分类判断，与路径规划前的预处理，可以使得规划的路径更为合理安全。而动态编队需要的高精度的队形保持与跟踪，通常由集群***中的各单体之间获取位置信息后保持相对位置，但是这样的方式需要时刻获取集群***中其余单体之间的状态信息，在通信延时或者短暂丢失均会对编队队形与精度造成影响。因此改进动态编队中的编队信息的获取方式，提升复杂曲线编队精度是一个急需解决的问题。

发明内容

本发明的目的是提供一种集群***动态编队的队形控制方法，以改进无人机、无人车、无人船等集群***编队队形精度不高的问题。

为了解决以上问题，本发明通过以下技术方案实现：

一种集群***动态编队的队形控制方法，包括：步骤S1、建立环境障碍威胁地图。步骤S2、根据所述环境障碍威胁地图为集群***中的领航主机获取最优路径。步骤S3、对所述最优路径进行参数化处理，得到第一路径信息。步骤S4、对所述第一路径信息进行优化处理得到第二路径信息，所述第二路径信息满足预设的运动学约束条件与避障要求条件。步骤S5、根据所述第二路径信息为所述集群***中的每个跟随从机分配独立的跟随路径，并将所述跟随路径参数化后再通过通信数据链在所述集群***中同步。步骤S6、所述领航主机以恒定速度V_d跟随所述第二路径信息，若在所述集群***内通信丢失且无法获得所述集群***的状态信息时，跟随从机也以所述恒定速度V_d跟随路径来保持编队；若在所述集群***内通信正常时，则在所述恒定速度V_d的基础上，通过速度协调控制器得到跟随从机的协调速度V_r。

优选地，还包括：步骤S7、所述集群***通过搭载的敏感器在运动过程中实时探测环境信息，探测到新的障碍物后，为所述障碍物设定影响半径r_i，为进入此影响半径r_i的集群单体进行避障控制，在所述恒定速度V_d或者所述协调速度V_r基础上，增加避障加速度a_i。

优选地，还包括：步骤S8、在编队过程中，根据环境复杂性与地面控制输入信息，定周期或者变周期搜索新的所述最优路径，并根据最新的所述最优路径重复步骤S3～S7。

优选地，所述步骤S1包括：将从起点到终点的方形区域划分为n*n的网格，每个所述网格为一个节点。

基于障碍类型确定不可通过区域，根据所述障碍类型的距离方位信息为每一网格设定不同的威胁系数，将每个所述威胁系数进行叠加处理后得到一个n*n的障碍威胁系数矩阵。

优选地，采用启发式算法获取所述最优路径：以叠加了所述障碍威胁系数矩阵中相应的障碍威胁系数与到终点的距离函数作为启发值。以从起点按照最小启发值路径到达当前位置的路径长度作为耗费值。将所述耗费值与所述启发值之和作为每一所述节点的代价值。从起点开始计算当前节点的周围节点的代价值，并从其中最小代价值点继续搜索，直到达到目标点，将各最小代价值点连接，得到所述最优路径。

优选地，所述第一路径信息H(x)采用如下公式进行计算：

式中，x_i和y_i分别是第i个原始规划路径点的x轴坐标和y轴坐标；q_i表示原始规划路径点i的导数；x_i+1和y_i+1分别是第i+1个原始规划路径点的x轴坐标和y轴坐标；q_i+1表示第i+1个原始规划路径点的导数。

优选地，所述步骤S4包括：

步骤S4.1、在第一路径信息中发生碰撞的下一路径点，新增该路径点前后两条原始路径点的直线连线中点。

步骤S4.2、再重新搜索新的路径得到所述第二路径信息。

步骤S4.3、重复所述步骤S4.1～步骤S4.2，直到所述第二路径信息满足所述运动学约束与避障要求。

优选地，所述步骤S5包括：以所述领航主机的质心为原点，速度切线方向为x轴，法线方向为y轴，在领航主机的所述第二路径信息的基础上叠加坐标偏移量，得到每一所述跟随从机的参数化的所述跟随路径。

所述坐标偏移量d_ij采用如下公式进行表示：

d_ij＝(x_{off_ij},y_{off_ij},z_{off_ij})

式中，x_{off_ij}表示表示集群单机i到j之间的x坐标向量，y_{off_ij}表示表示集群单机i到j之间的y坐标向量，z_{off_ij}表示表示集群单机i到j之间的z坐标向量。

优选地，所述步骤S6包括：所述跟随从机的协调速度V_r通过如下公式计算：

式中，||V_d||∈(v_min+Δv,v_max-Δv)，f(x)＝2/(1+e^(-x))-1，f(x)∈(-1,1)，最终的协调速度满足V_r∈(v_min,v_max)，v_min表示集群单机的最小运动速度；v_max表示集群单机的最大运动速度；Δv表示可调速度量大小；e^(-x)表示表示自然指数e的-x次方。

优选地，所述步骤S7包括：所述避障加速度a_i通过如下公式计算：

式中，b和c为控制力系数，ρ_ij为集群单体i和障碍j的距离；所述避障加速度a_i的方向由障碍点指向所述集群单体i。

当探测的所述新的障碍物存在运动速度时，需要调整避障加速度a_i的方向。

调整方法为当进入所述障碍物的影响半径r_i的集群单体与所述障碍物的运动速度矢量相交时：从受影响的所述集群单体速度方向观察，如果所述集群单体的跟随路径在受影响的所述集群单体的右方，则将避障加速度a_i的方向逆时针转动90度。从受影响的所述集群单体速度方向观察，如果所述集群单体的跟随路径在受影响的所述集群单体的左方，则将避障加速度a_i的方向顺时针转动90度。

本发明至少具有以下优点之一：

本发明相比于传统的编队控制方法，本发明通过将障碍信息分类，引入威胁系数假设，因此规划的路径信息更合理安全；对编队过程中的动态障碍，根据障碍运动方向进行合理的优化规避，路径更优；将搜索的路径信息以分段的参数化数据描述并周期性更新，可以保证在丢失通信的一段时间内编队队形不受影响，且参数化数据可以实现对编队路径精准的描述，编队控制器可以达到更高的编队控制精度。因此可以降低通信延时与短暂丢失对编队队形的干扰，并大大提升集群***跟踪复杂曲线路径的精度与稳定性。本发明应用于无人机、无人车、无人船等集群***的高精度编队队形控制。

附图说明

图1是本发明一实施例提供的一种集群***动态编队的队形控制方法流程图；

图2是本发明一实施例提供的分段曲线公共点导数求解示意图；

图3是本发明一实施例提供的参数化路径优化调整示意图；

图4是本发明一实施例提供的集群编队队形坐标系示意图；

图5是本发明一实施例提供的动态障碍避障加速度调整方向示意图。

具体实施方式

以下结合附图和具体实施方式对本发明提出的一种集群***动态编队的队形控制方法作进一步详细说明。根据下面说明，本发明的优点和特征将更清楚。需要说明的是，附图采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施方式的目的。为了使本发明的目的、特征和优点能够更加明显易懂，请参阅附图。须知，本说明书所附图式所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容能涵盖的范围内。

如图1所示，本实施例提供的一种集群***动态编队的队形控制方法，包括：步骤S1、建立环境障碍威胁地图。即建立环境障碍威胁地图，得到n*n的障碍威胁系数矩阵。所述步骤S1包括：将从起点到终点的方形区域划分为n*n的网格。每个所述网格为一个节点；每一所述网格的大小根据集群***具体组成单元的物理尺寸与编队精度确定。

基于障碍类型(障碍物类型)确定不可通过区域(例如将树木、建筑等障碍的物理空间范围设为不可通过区域)，根据所述障碍类型(不同障碍的)的距离方位信息为每一网格设定不同的威胁系数，将每个所述威胁系数进行叠加处理后得到一个n*n的障碍威胁系数矩阵(将为每个网格内的障碍物分配的威胁系数叠加处理后得到一个n*n的障碍威胁系数矩阵)。离障碍(障碍物)越近威胁系数越大，表明路径规划算法越应该避开此区域。例如对于辐射源障碍，各障碍的物理空间不可通过区域大小相等，但是辐射强度不同，因此通过为辐射强度更大的辐射源障碍设定更大的威胁系数，可以在两个不同强度辐射源之间规划更为合理的路径。

步骤S2、根据所述环境障碍威胁地图为集群***中的领航主机获取最优路径(基于环境障碍威胁地图为领航主机搜索最优路径)。

所述步骤S2包括：在进行最优路径搜索时，只考虑目前已知的环境障碍信息，不考虑后续集群***的敏感器探测到的障碍。避开这些障碍到达终点存在多条可用的路径，但是考虑到集群***的性能约束，需要寻找一条最优路径。

采用启发式算法搜索所述最优路径：以叠加了所述障碍威胁系数矩阵中相应的障碍威胁系数与到终点的距离函数作为启发值h(n)：

式中，a为权重系数，w_i为第i个障碍的威胁系数，i＝1,2,…,k，d_max是路径搜索过程中集群单体(例如：无人机)与障碍中心距离可能的最大值，d(pⁱ)是集群单体与第i个障碍的中心距离，x，y为当前搜索位置坐标，x_d，y_d为终点坐标，n表示第n个节点。

以从起点按照最小启发值h(n)路径到达当前位置的路径长度作为耗费值，耗费值与启发值之和为每一所述节点的代价值。从起点开始计算周围节点(周围节点是指当前节点上下左右以及4个斜对角一共8个所述节点。)的代价值，并从其中最小代价值点继续搜索，直到达到目标点，将各最小代价值点连接即可得到搜索的所述最优路径。

即，所述步骤S2基于威胁地图为领航主机进行最优路径搜索，综合考虑最短路径与最小障碍威胁获得综合的最优路径。

步骤S3、对所述最优路径进行参数化处理，得到第一路径信息(参数化规划路径信息)。

上述得到的最优路径是离散化的，且各离散路径是由分段折线组成，离散路径可能存在较大转角。因此将上述最优路径分段采用多项式拟合后实现参数化，为了保证集群***在满足运动学约束的前提下跟踪规划路径，参数化后的路径需要满足以下2点：第一点为经过原始路径点；第二点为参数化后的各分段路径在原始公共路径点处导数相同。如图2所示，参数化后的路径在原始路径点处的切线与离散路径点折线的角平分线垂直，可由此求出该导数。参数后的路径(第一路径信息)H(x)由此求出：

式中，x_i和y_i分别是第i个原始规划路径点的x轴坐标和y轴坐标；q_i表示原始规划路径点i的导数；x_i+1和y_i+1分别是第i+1个原始规划路径点的x轴坐标和y轴坐标；q_i+1表示第i+1个原始规划路径点的导数。

即，所述步骤S3通过多项式拟合，将上述路径进行参数化，以便通过通信网传播。

步骤S4、对所述第一路径信息进行优化处理得到第二路径信息(参数化后的路径信息再优化)，所述第二路径信息满足预设的运动学约束条件与避障要求条件。

具体的，运动学约束条件：||a||≤a_max,||v||≤v_max,v、a和λ分别为集群单机(例如无人机)速度、加速度和转弯半径，v_max、a_max和λ_max分别为最大速度、最大加速度和最大转弯半径。避障要求条件：d＜R_min。d和R_min分别是集群单机与障碍的距离和障碍的影响半径，进入此半径会发生碰撞。

所述步骤S4包括：如图3所示，参数化后的路径可能存在与障碍物再次碰撞的可能。采用如下优化方案：

步骤S4.1、在第一路径(第一路径信息)中发生碰撞的下一路径点，新增该路径点前后两条原始路径点的直线连线中点。

步骤S4.2、再重新搜索新的路径得到所述第二路径(第二路径信息)。

这里搜索就是增加两个路径点后按照上述公式H(x)重新进行计算得到新的第二路径信息。

步骤S4.3、重复所述步骤S4.1～步骤S4.2，直到所述第二路径信息满足所述运动学约束与避障要求。

即，所述步骤S4是将上述参数化后的路径信息再优化，直到优化后的曲线满足运动学约束与避障要求。

步骤S5、根据所述第二路径信息为所述集群***中的每个跟随从机分配独立的跟随路径，并将所述跟随路径参数化后再通过通信数据链在所述集群***中同步(集群***的编队队形分配)。

所述步骤S5包括：在上述为领航主机搜索获得的参数化路径基础上，增加坐标偏移量，为集群***中每个跟随从机分配独立的跟随路径，然后将此跟随路径的分段曲线参数以参数矩阵的形式通过通信数据链在集群***中同步。

如图4所示，以领航主机质心为原点，速度切线方向为x轴，法线方向为y轴，在所述领航主机的第二路径信息的基础上叠加坐标偏移量d_ij＝(x_{off_ij},y_{off_ij},z_{off_ij})，得到每一所述跟随从机的参数化的所述跟随路径(得到其余跟随从机的路径)；

所述坐标偏移量d_ij采用如下公式进行表示：

d_ij＝(x_{off_ij},y_{off_ij},z_{off_ij}) (3)

式中，x_{off_ij}表示表示集群单机i到j之间的x坐标向量，y_{off_ij}表示表示集群单机i到j之间的y坐标向量，z_{off_ij}表示表示集群单机i到j之间的z坐标向量。

即，所述步骤S5通过编队信息为其余跟随从机分配跟随路径并参数化。所述编队信息指的是集群***的队形信息，即集群***应该按照什么样的队形布置，如一字型，人字形，五角星型等。编队信息是通过坐标偏移量d_ij来体现的。

步骤S6、编队路径协调控制，所述领航主机以恒定速度V_d跟随编队路径(第二路径信息)，若在所述集群***内通信丢失且无法获得集群***的状态信息时，跟随从机也以所述恒定速度V_d跟随路径来保持编队；若在所述集群***内通信正常时，则在所述恒定速度V_d的基础上，通过速度协调控制器得到跟随从机的协调速度V_r，以进一步提高编队精度。即，所述步骤S6是根据通信条件，通过速度控制量为编队路径完成队形协调控制。

所述集群***的状态信息指集群中各单机的姿态、位置、速度、数据链信号强度、电量、任务进度等信息。

所述跟随从机的协调速度V_r通过如下公式计算：

式中，||V_d||∈(v_min+Δv,v_max-Δv)，f(x)＝2/(1+e^(-x))-1，f(x)∈(-1,1)，最终的协调速度满足V_r∈(v_min,v_max)，v_min表示集群单机的最小运动速度；v_max表示集群单机的最大运动速度；Δv表示可调速度量大小；e^(-x)表示表示自然指数e的-x次方。

本实施例还包括：步骤S7、障碍避障控制，即所述集群***通过搭载的敏感器在运动过程中实时探测环境信息，探测到新的障碍物后，为所述障碍物设定影响半径r_i，为进入此影响半径r_i的集群单体进行避障控制，在所述恒定速度V_d或者所述协调速度V_r基础上，增加避障加速度a_i。

所述避障加速度a_i通过如下公式计算：

式中，b和c为控制力系数，ρ_ij为集群单体i和障碍j的距离；所述避障加速度a_i的方向由障碍点指向所述集群单体i。

如图5所示，图a、b情况下斥力逆时针转动90度；图c、d情况下顺时针转动90度。

具体的，当探测的所述新的障碍物存在运动速度时，需要调整避障加速度a_i的方向。

调整方法为当进入所述障碍物的影响半径r_i的集群单体与所述障碍物的运动速度矢量相交时：

从受影响的所述集群单体速度方向观察，如果目标路径(所述集群单体的跟随路径)在受影响的所述集群单体的右方，则将避障加速度a_i的方向逆时针转动90度(则将避障加速度乘以90度方向余弦矩阵R(90))。

从受影响的所述集群单体速度方向观察，如果目标路径(所述集群单体的跟随路径)在受影响的所述集群单体的左方，则将避障加速度a_i的方向顺时针转动90度(则将避障加速度乘以-90度方向余弦矩阵R(-90))。

即，所述步骤S7是对编队过程中后加入的障碍完成避障控制。

本实施例还包括：步骤S8、更新路径信息，即在编队过程中，根据环境复杂性与地面控制输入信息，定周期或者变周期搜索新的所述最优路径，并根据最新的所述最优路径重复步骤S3～S7(并将最新的路径参数化矩阵在集群***中更新同步)。所述步骤S8是指周期更新路径信息，并将最新的路径参数化矩阵在集群***中更新同步。

本实施例研究的对象基于动态编队。动态编队通过数据链可以在运动过程中根据地面控制输入与地理环境信息，实时改变编队队形与航迹，因此编队更加灵活，具备更强的适用性，但是通信延迟、丢失，带宽以及外界干扰都会影响编队精度。由此可知，本实施例是一种适用于无人机、无人车、无人船等集群***在复杂空间，避开障碍情况下实现复杂曲线路径跟踪的高精度编队队形控制方法。

本实施例通过将障碍信息分类，引入威胁系数假设，因此规划的路径信息更合理安全；对编队过程中的动态障碍，根据障碍运动方向进行合理的优化规避，路径更优；将搜索的路径信息以分段的参数化数据描述并周期性更新，可以保证在丢失通信的一段时间内编队队形不受影响，且参数化数据可以实现对编队路径精准的描述，编队控制器可以达到更高的编队控制精度。因此可以降低通信延时与短暂丢失对编队队形的干扰，并大大提升集群***跟踪复杂曲线路径的精度与稳定性。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

应当注意的是，在本文的实施方式中所揭露的装置和方法，也可以通过其他的方式实现。以上所描述的装置实施方式仅仅是示意性的，例如，附图中的流程图和框图显示了根据本文的多个实施方式的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用于执行规定的功能或动作的专用的基于硬件的***来实现，或者可以用专用硬件与计算机指令的组合来实现。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (7)

1.一种集群***动态编队的队形控制方法，其特征在于，包括：

步骤S1、建立环境障碍威胁地图；

步骤S2、根据所述环境障碍威胁地图为集群***中的领航主机获取最优路径；

步骤S3、对所述最优路径进行参数化处理，得到第一路径信息；

所述第一路径信息H(x)采用如下公式进行计算：

式中，x_i和y_i分别是第i个原始规划路径点的x轴坐标和y轴坐标；q_i表示原始规划路径点i的导数；x_i+1和y_i+1分别是第i+1个原始规划路径点的x轴坐标和y轴坐标；q_i+1表示第i+1个原始规划路径点的导数；

步骤S4、对所述第一路径信息进行优化处理得到第二路径信息，所述第二路径信息满足预设的运动学约束条件与避障要求条件；

步骤S5、根据所述第二路径信息为所述集群***中的每个跟随从机分配独立的跟随路径，并将所述跟随路径参数化后再通过通信数据链在所述集群***中同步；

步骤S6、所述领航主机以恒定速度V_d跟随所述第二路径信息，若在所述集群***内通信丢失且无法获得所述集群***的状态信息时，跟随从机也以所述恒定速度V_d跟随路径来保持编队；若在所述集群***内通信正常时，则在所述恒定速度V_d的基础上，通过速度协调控制器得到跟随从机的协调速度V_r；

所述步骤S6包括：所述跟随从机的协调速度V_r通过如下公式计算：

式中，||V_d||∈(v_min+Δv,v_max-Δv)，f(x)＝2/(1+e^(-x))-1，f(x)∈(-1,1)，最终的协调速度满足V_r∈(v_min,v_max)，v_min表示集群单机的最小运动速度；v_max表示集群单机的最大运动速度；Δv表示可调速度量大小；e^(-x)表示表示自然指数e的-x次方；

步骤S7、所述集群***通过搭载的敏感器在运动过程中实时探测环境信息，探测到新的障碍物后，为所述障碍物设定影响半径r_i，为进入此影响半径r_i的集群单体进行避障控制，在所述恒定速度V_d或者所述协调速度V_r基础上，增加避障加速度a_i。

2.如权利要求1所述的集群***动态编队的队形控制方法，其特征在于，还包括：步骤S8、在编队过程中，根据环境复杂性与地面控制输入信息，定周期或者变周期搜索新的所述最优路径，并根据最新的所述最优路径重复步骤S3～S7。

3.如权利要求2所述的集群***动态编队的队形控制方法，其特征在于，所述步骤S1包括：

将从起点到终点的方形区域划分为n*n的网格，每个所述网格为一个节点；

基于障碍类型确定不可通过区域，根据所述障碍类型的距离方位信息为每一网格设定不同的威胁系数，将每个所述威胁系数进行叠加处理后得到一个n*n的障碍威胁系数矩阵。

4.如权利要求3所述的集群***动态编队的队形控制方法，其特征在于，

采用启发式算法获取所述最优路径：以叠加了所述障碍威胁系数矩阵中相应的障碍威胁系数与到终点的距离函数作为启发值；

以从起点按照最小启发值路径到达当前位置的路径长度作为耗费值；

将所述耗费值与所述启发值之和作为每一所述节点的代价值；

从起点开始计算当前节点的周围节点的代价值，并从其中最小代价值点继续搜索，直到达到目标点，将各最小代价值点连接，得到所述最优路径。

5.如权利要求4所述的集群***动态编队的队形控制方法，其特征在于，所述步骤S4包括：

步骤S4.1、在第一路径信息中发生碰撞的下一路径点，新增该路径点前后两条原始路径点的直线连线中点；

步骤S4.2、再重新搜索新的路径得到所述第二路径信息；

步骤S4.3、重复所述步骤S4.1～步骤S4.2，直到所述第二路径信息满足所述运动学约束与避障要求。

6.如权利要求5所述的集群***动态编队的队形控制方法，其特征在于，所述步骤S5包括：

以所述领航主机的质心为原点，速度切线方向为x轴，法线方向为y轴，在领航主机的所述第二路径信息的基础上叠加坐标偏移量，得到每一所述跟随从机的参数化的所述跟随路径；

所述坐标偏移量d_ij采用如下公式进行表示：

d_ij＝(x_{off_ij},y_{off_ij},z_{off_ij})

式中，x_{off_ij}表示表示集群单机i到j之间的x坐标向量，y_{off_ij}表示表示集群单机i到j之间的y坐标向量，z_{off_ij}表示表示集群单机i到j之间的z坐标向量。

7.如权利要求1所述的集群***动态编队的队形控制方法，其特征在于，所述步骤S7包括：所述避障加速度a_i通过如下公式计算：

式中，b和c为控制力系数，ρ_ij为集群单体i和障碍j的距离；所述避障加速度a_i的方向由障碍点指向所述集群单体i；

当探测的所述新的障碍物存在运动速度时，需要调整避障加速度a_i的方向；

调整方法为当进入所述障碍物的影响半径r_i的集群单体与所述障碍物的运动速度矢量相交时：

从受影响的所述集群单体速度方向观察，如果所述集群单体的跟随路径在受影响的所述集群单体的右方，则将避障加速度a_i的方向逆时针转动90度；

从受影响的所述集群单体速度方向观察，如果所述集群单体的跟随路径在受影响的所述集群单体的左方，则将避障加速度a_i的方向顺时针转动90度。

Images