CN115993845A

CN115993845A - 一种集群智能系统协同运动规划与编队控制方法

Info

Publication number: CN115993845A
Application number: CN202310287976.0A
Authority: CN
Inventors: 赵宇; 杨澜; 刘永芳; 周媛
Original assignee: Northwestern Polytechnical University; Shenzhen Institute of Northwestern Polytechnical University
Current assignee: Northwestern Polytechnical University; Shenzhen Institute of Northwestern Polytechnical University
Priority date: 2023-03-23
Filing date: 2023-03-23
Publication date: 2023-04-21
Anticipated expiration: 2043-03-23
Also published as: CN115993845B

Abstract

本发明公开了一种集群智能系统协同运动规划与编队控制方法，涉及集群智能系统技术领域。该方法包括获取所有智能体的初始状态信息并设置编队初始条件；根据所有智能体的目标编队构型和通信拓扑确定智能体下一时刻的期望位置；根据集群智能系统的动力学方程，建立智能体位置预测模型；根据智能体下一时刻的期望位置，建立编队控制目标函数；以智能体运动的加速度及运动空间区间作为约束条件对编队控制目标函数进行优化，构建当前时刻的预测轨迹；采用带有软约束的按需避碰策略对编队控制目标函数的优化过程进行迭代求解，确定无碰撞轨迹并控制所有智能体进行编队。本发明实现了在保证避碰条件下的运动轨迹规划，同时按照规划的路径完成编队任务。

Description

一种集群智能系统协同运动规划与编队控制方法

技术领域

本发明涉及集群智能系统技术领域，具体涉及一种集群智能系统协同运动规划与编队控制方法。

背景技术

近年来，集群智能系统出现在许多工程领域，这些系统的控制问题越来越受到人们的重视。协同控制的目标是使多个自治智能体通过局部交互进行有效地协同工作，实现集体群体行为。在执行大规模的复杂任务时，协同移动的集群智能系统具有较高的工作效能和较强的容错能力。

运动规划与控制是集群智能系统协同控制研究中的一个重要分支。由于其他智能体正在规划并同时执行其轨迹，这就要求运动规划算法要具有迅速响应复杂环境变化的能力，在避免与其他智能体发生碰撞的同时完成编队控制任务。因此，如何实现在共享环境中执行任务并生成无碰撞轨迹是本领域值得研究的问题。

发明内容

针对现有技术中的上述不足，本发明提供了一种集群智能系统协同运动规划与编队控制方法，以实现集群智能系统在规定的区域内规划出安全无碰撞的路径并实现编队控制任务。

为了达到上述发明目的，本发明采用的技术方案为：

一种集群智能系统协同运动规划与编队控制方法，包括以下步骤：

S1、获取所有智能体的初始状态信息并设置编队初始条件；

S2、根据所有智能体的目标编队构型和通信拓扑确定智能体下一时刻的期望位置；

S3、根据集群智能系统的动力学方程，建立智能体位置预测模型；

S4、根据智能体下一时刻的期望位置和智能体位置预测模型，建立编队控制目标函数；

S5、以智能体运动的加速度及运动空间区间作为约束条件对编队控制目标函数进行优化，构建当前时刻的预测轨迹；

S6、采用带有软约束的按需避碰策略对编队控制目标函数的优化过程进行迭代求解，确定无碰撞轨迹并控制所有智能体进行编队。

可选地，步骤S1中：

初始状态信息具体包括初始位置信息和初始速度信息；

编队初始条件具体包括时间序列、智能体间的安全距离、时间步长参数、预测时间步及预测视界长度。

可选地，步骤S2中智能体下一时刻的期望位置表示为：

其中，和分别为智能体 i在 t _z时刻和 t _z+1时刻的位置信息，为智能体 j在 t _z时刻的位置信息， h _i和 h _j分别为智能体 i和智能体 j的期望编队构型常数，为智能体 i的相邻智能体的集合，为智能体 i的相邻智能体的个数。

可选地，步骤S3中智能体位置预测模型表示为：

其中，为智能体 i在 t _z时刻的预测位置序列，，为智能体 i到达 t _z时刻的第 k _t预测时间步的初始状态，为智能体 i达到 t _z时刻的第 k _t预测时间步的预测输入序列，，，，，，和分别为单位矩阵和零矩阵，为时间步长参数， K为预测视界长度。

可选地，步骤S4中编队控制目标函数具体为：

其中，为编队控制目标函数，为智能体 i在 t _z时刻上的预测输入序列，为智能体 i在 t _z时刻的轨迹误差，为智能体 i在 t _z时刻的控制量，为智能体 i在 t _z时刻的输入变化量，和分别为智能体 i在 t _z时刻和 t _z+1时刻的位置信息，为正定块对角矩阵， T为矩阵转置符号，为控制量的惩罚权重系数，，和分别为单位矩阵和零矩阵，，为智能体 i到达 t _z时刻的第 k _t-1预测时间步应用的实际输入信息，为零矩阵。

可选地，步骤S5中以智能体运动的加速度及运动空间区间作为约束条件具体为：

其中，为智能体 i达到 t _z时刻的第 k _t预测时间步的预测输入序列，，分别为最小输入序列和最大输入序列，，分别为运动空间的最小位置序列和最大位置序列，，，，，，和分别为单位矩阵和零矩阵，为时间步长参数， K为预测视界长度，，为智能体 i到达 t _z时刻的第 k _t预测时间步的初始状态。

可选地，步骤S6中带有软约束的按需避碰策略具体为：

当智能体预测在第预测时间步发生碰撞时，在优化问题中引入碰撞约束及松弛变量，表示为：

其中，，为对角矩阵，和分别为智能体 i和智能体 j利用第预测时间步的可用信息得到的第预测时间步的预测位置信息，为智能体 i利用第预测时间步的可用信息得到的第预测时间步的预测位置信息， n为模拟边界的阶数，表示在第预测时间步上检测到发生碰撞的预测视界，，为2范数，为智能体间的安全距离，为决策变量。

可选地，步骤S6中采用带有软约束的按需避碰策略对编队控制目标函数的优化过程进行迭代求解具体为：

其中，为代价函数，，为编队控制目标函数，，为所有决策变量的堆叠向量，，为标量优化参数，分别为零矩阵、全一矩阵和单位矩阵， T为矩阵转置符号，和分别为智能体 i在初始时刻 t ₀的位置信息和智能体 i在预测视界为1的位置信息预测值，为智能体 i设定的初始位置，和分别为智能体 i利用第 k _t预测时间步的可用信息得到的第预测时间步的状态预测值和加速度预测值，表示在第预测时间步上的预测视界，为智能体 i利用第 k _t预测时间步的可用信息得到的第预测时间步的状态预测值，，，，，，和分别为单位矩阵和零矩阵，为时间步长参数， K为预测视界长度，为智能体 i达到 t _z时刻的第 k _t预测时间步的预测输入序列，，分别为最小输入序列和最大输入序列，，分别为运动空间的最小位置序列和最大位置序列，，为在 k _t时刻的初始状态， k _t为预测时间步，，，，为智能体 i到达 t _z时刻的第 k _t预测时间步的初始状态，为对角矩阵， n为模拟边界的阶数，和分别为智能体 i和智能体 j利用第预测时间步的可用信息得到的第预测时间步的预测位置信息，，为2范数，为智能体间的安全距离，为决策变量，和分别为智能体 i在 t _z时刻和 t _z+1时刻的位置信息，为智能体 j在 t _z时刻的位置信息， h _i和 h _j分别为智能体 i和智能体 j的期望编队构型常数，为智能体 i的相邻智能体的集合，为智能体 i的相邻智能体的个数。

本发明具有以下有益效果：

本发明首先获取所有智能体的初始状态信息并设置编队初始条件；然后根据所有智能体的目标编队构型和通信拓扑确定智能体下一时刻的期望位置；再根据集群智能系统的动力学方程，建立智能体位置预测模型；从而根据智能体下一时刻的期望位置，建立编队控制目标函数；并以智能体运动的加速度及运动空间区间作为约束条件对编队控制目标函数进行优化，构建当前时刻的预测轨迹；最后采用带有软约束的按需避碰策略对编队控制目标函数的优化过程进行迭代求解，确定无碰撞轨迹并控制所有智能体进行编队。本发明实现了集群智能系统在保证避碰条件下的运动轨迹规划，同时按照规划的路径完成编队任务。

附图说明

图1所示为本发明实施例提供的一种集群智能系统协同运动规划与编队控制方法的流程示意图；

图2所示为本发明实施例中6架无人机的通信拓扑的示意图；

图3所示为本发明实施例中无人机集群系统位置随时间变化的示意图；

图4所示为本发明实施例中无人机集群系统编队的移动轨迹的示意图；

图5所示为本发明实施例中无人机集群系统中无人机1与其他无人机间的相对距离的示意图；

图6所示为本发明实施例中无人机集群系统编队一致性误差的示意图；

图7所示为本发明实施例中无人机集群系统加速度变化的示意图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

如图1所示，本发明实施例提供的一种集群智能系统协同运动规划与编队控制方法，包括以下步骤S1至S6：

S1、获取所有智能体的初始状态信息并设置编队初始条件；

在本发明的一个可选实施例中，本实施例针对的集群智能系统为多个可移动的线性动力学的智能体组成的系统，首先需要获取所有智能体的初始状态信息并设置编队初始条件，其中初始状态信息具体包括初始位置信息和初始速度信息；编队初始条件具体包括时间序列、智能体间的安全距离、时间步长参数、预测时间步及预测视界长度。

具体而言，时间序列为，其中，并且初始时刻；智能体间的安全距离为；时间步长参数；预测时间步，并且；预测视界离散化时间指标为。

在本发明的一个可选实施例中，本实施例定义目标编队构型，其中 T为矩阵转置符号；通信拓扑具体为每一个智能体只与其相邻智能体互相通信且具有生成树，从而确定智能体下一时刻的期望位置表示为：

其中，和分别为智能体 i在 t _z时刻和 t _z+1时刻的位置信息，为智能体 j在 t _z时刻的位置信息， h _i和 h _j分别为智能体 i和智能体 j的期望编队构型常数，为智能体 i的相邻智能体的集合，为智能体 i的相邻智能体的个数， N为智能体的总数。

在本发明的一个可选实施例中，本实施例的集群智能系统的动力学方程为：

其中，分别为在时间步长为 k时刻下智能体 i的离散化轴方向上的位置、速度和加速度。本实施例以加速度作为输入，离散化步长选取为。

根据集群智能系统的动力学方程，建立一个线性模型来表示在固定视界长度 K上的智能体的状态，智能体 i的动力学模型表示为：

其中，和分别为维度为3的单位矩阵和零矩阵，为时间步长参数，、和分别为智能体 i利用第 k _t预测时间步的可用信息得到的第预测时间步的位置信息预测值、速度信息预测值和加速度信息预测值。

从而得到智能体位置预测模型表示为：

其中，为智能体 i在 t _z时刻的预测位置序列，，为智能体 i到达 t _z时刻的第 k _t预测时间步的初始状态，为智能体 i达到 t _z时刻的第 k _t预测时间步的预测输入序列，，为智能体 i在第 k _t预测时间步进行预测时的第一个预测视界上的预测输入；为智能体 i利用第 k _t预测时间步的可用信息得到的第预测时间步的预测加速度信息，即智能体 i在第 k _t预测时间步进行预测时的第二个预测视界上的预测输入；为智能体 i利用第 k _t预测时间步的可用信息得到的第预测时间步的预测加速度信息，即智能体 i在第 k _t预测时间步进行预测时的第 K个预测视界上的预测输入；，，，，，，，和分别为单位矩阵和零矩阵，为时间步长参数， K为预测视界长度。

S4、根据智能体下一时刻的期望位置，建立编队控制目标函数；

在本发明的一个可选实施例中，本实施例以轨迹误差、控制量和输入变化量建立编队控制目标函数；其中轨迹误差项旨在最小化视界上最后个时间步的预测位置与下一时刻期望位置之间的误差之和，表示为：

其中，为智能体 i利用第 k _t预测时间步的可用信息得到的第预测时间步的预测位置信息，为智能体 i在时刻的位置信息， K为预测视界长度。

上式可以转化为含输入序列的二次代价函数，表示为：

其中，是一个正定块对角矩阵，在每个时间步长上对误差进行加权；为智能体 i到达 t _z时刻的第 k _t预测时间步的预测位置序列，为智能体 i在 t _z+1时刻的位置序列。

控制量项旨在利用二次代价函数来最小化控制量，表示为：

其中，是一个正定块对角矩阵，；为控制量的惩罚权重系数。

输入变化量项用于最小化加速度的变化，从而使输入轨迹平滑，表示为：

其中，和分别为智能体 i利用第 k _t预测时间步的可用信息得到的第预测时间步和第预测时间步的预测加速度信息， K为预测视界长度。

上式可以转化为含输入序列的二次代价函数，表示为：

其中，，，是一个正定块对角矩阵，，为输入变化量的惩罚权重系数。

根据上述轨迹误差、控制量和输入变化量建立编队控制目标函数，表示为：

其中，为编队控制目标函数，为智能体 i在 t _z时刻上的预测输入序列，为智能体 i在 t _z时刻的轨迹误差，为智能体 i在 t _z时刻的控制量，为智能体 i在 t _z时刻的输入变化量，和分别为智能体 i在 t _z时刻和 t _z+1时刻的位置信息，为正定块对角矩阵， T为矩阵转置符号，为控制量的惩罚权重系数，，和分别为单位矩阵和零矩阵，，为智能体 i到达 t _z时刻的第 k _t-1预测时间步应用的实际输入信息，具体是指实际应用的加速度信息，为零矩阵。

在本发明的一个可选实施例中，本实施例建立的优化问题是基于没有检测到任何未来碰撞的情况下建立的，目标代价函数为，约束条件限制了智能体运动的最小和最大加速度及运动空间，表示为

其中，为智能体 i达到 t _z时刻的第 k _t预测时间步的预测输入序列，，分别为最小输入序列和最大输入序列，，分别为运动空间的最小位置序列和最大位置序列，，，，，，，，和分别为单位矩阵和零矩阵，为时间步长参数， K为预测视界长度，，为智能体 i到达 t _z时刻的第 k _t预测时间步的初始状态。

在本发明的一个可选实施例中，本实施例采用的带有软约束的按需避碰策略具体为：

其中，为对角矩阵来模拟智能体的碰撞边界，为智能体 i利用第预测时间步的可用信息得到的第预测时间步的预测位置信息，为智能体 j利用第预测时间步的可用信息得到的第预测时间步的预测位置信息， n为模拟边界的阶数，通常取2，表示在预测时刻上检测到发生碰撞的某一个预测视界，为智能体间的安全距离，为决策变量，为矩阵范数。

上式关于智能体 i在处泰勒级数展开，得到

其中，，。

将碰撞约束包含在智能体的子集内，表示为

其中，模拟了智能体周围的半径，将限制为智能体 i半径内的邻居子集，目的是在优化问题中安全地减少碰撞约束的数量。

本实施例采用带有软约束的按需避碰策略对编队控制目标函数的优化过程进行迭代求解具体为：

其中，为代价函数，，为编队控制目标函数，，为所有决策变量的堆叠向量，下标aug表示使用的是增广状态矩阵，，为标量优化参数，分别为零矩阵、全一矩阵和单位矩阵， T为矩阵转置符号，和分别为智能体 i在初始时刻 t ₀的位置信息和智能体 i在预测视界为1的位置信息预测值，为智能体 i设定的初始位置，和分别为智能体 i利用预测 k _t时刻的可用信息得到的时刻的状态预测值和加速度预测值，表示在预测时刻上的某一个预测视界，为智能体 i利用第 k _t预测时间步的可用信息得到的第预测时间步的状态预测值，，，，，，和分别为单位矩阵和零矩阵，为时间步长参数， K为预测视界长度，为智能体 i达到 t _z时刻的第 k _t预测时间步的预测输入序列，，分别为最小输入序列和最大输入序列，，分别为运动空间的最小位置序列和最大位置序列，，为在 k _t时刻的初始状态， k _t为预测时间步，，，为对角矩阵， n为模拟边界的阶数，和分别为智能体 i和智能体 j利用第预测时间步的可用信息得到的第预测时间步的预测位置信息，，为智能体间的安全距离，为决策变量，和分别为智能体 i在 t _z时刻和 t _z+1时刻的位置信息，为智能体 j在 t _z时刻的位置信息， h _i和 h _j分别为智能体 i和智能体 j的期望编队构型常数，为智能体 i的相邻智能体的集合，为智能体 i的相邻智能体的个数。

下面以具体实例对本实施例提供的一种集群智能系统协同运动规划与编队控制方法进行具体分析说明。

考虑在二维平面移动的6架无人机组成的集群系统，图2给出了无人机间的有向通信拓扑。设置无人机间的安全距离为，，时间步长参数，，视界预测长度。此外，每个无人机的初始z位置保持在同一平面上，初始 x、 y位置及编队构型如下表1：

表1

如果无人机 i利用时刻的预测信息没有检测到任何未来的碰撞，那么它通过求解下式来更新其输入序列：

如果无人机 i利用时刻的预测信息检测到了未来的碰撞，那么它通过求解下式来更新其输入序列：

图3和图4分别展示了无人机集群系统的运动过程及运动轨迹，可以看出无人机1与无人机2、无人机3与无人机6发生了位置的交互并生成了各自安全的无碰撞轨迹，并且所有的无人机实现了协同编队并在之后保持编队构型。

以无人机1为例说明本实施例的避碰效果，如图5所示，虚线表示无人机间的安全距离，无人机1与其他无人机之间的相对距离总是大于安全距离。

图6展示了6个无人机的一致性误差随时间的变化情况，在时均收敛到0，达到了期望的编队构型。

由图7可以看出，每个无人机的输入均落在范围内，满足设置的输入约束。从上述结果可以看出，无人机集群系统在所设计的协同运动规划与编队控制下，可以找到安全无碰撞的路径并完成期望的编队任务。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims

1.一种集群智能系统协同运动规划与编队控制方法，其特征在于，包括以下步骤：

S1、获取所有智能体的初始状态信息并设置编队初始条件；

2.根据权利要求1所述的一种集群智能系统协同运动规划与编队控制方法，其特征在于，步骤S1中：

初始状态信息具体包括初始位置信息和初始速度信息；

3.根据权利要求1所述的一种集群智能系统协同运动规划与编队控制方法，其特征在于，步骤S2中智能体下一时刻的期望位置表示为：

其中，和分别为智能体i在t _z时刻和t _z+1时刻的位置信息，为智能体j在t _z时刻的位置信息，h _i和h _j分别为智能体i和智能体j的期望编队构型常数，为智能体i的相邻智能体的集合，为智能体i的相邻智能体的个数。

4.根据权利要求1所述的一种集群智能系统协同运动规划与编队控制方法，其特征在于，步骤S3中智能体位置预测模型表示为：

其中，为智能体i在t _z时刻的预测位置序列，，为智能体i到达t _z时刻的第k _t预测时间步的初始状态，为智能体i达到t _z时刻的第k _t预测时间步的预测输入序列，，，，，，和分别为单位矩阵和零矩阵，为时间步长参数，K为预测视界长度。

5.根据权利要求1所述的一种集群智能系统协同运动规划与编队控制方法，其特征在于，步骤S4中编队控制目标函数具体为：

其中，为编队控制目标函数，为智能体i在t _z时刻上的预测输入序列，为智能体i在t _z时刻的轨迹误差，为智能体i在t _z时刻的控制量，为智能体i在t _z时刻的输入变化量，和分别为智能体i在t _z时刻和t _z+1时刻的位置信息，为正定块对角矩阵，T为矩阵转置符号，为控制量的惩罚权重系数，，和分别为单位矩阵和零矩阵，，为智能体i到达t _z时刻的第k _t-1预测时间步应用的实际输入信息，为零矩阵。

6.根据权利要求1所述的一种集群智能系统协同运动规划与编队控制方法，其特征在于，步骤S5中以智能体运动的加速度及运动空间区间作为约束条件具体为：

其中，为智能体i达到t _z时刻的第k _t预测时间步的预测输入序列，，分别为最小输入序列和最大输入序列，，分别为运动空间的最小位置序列和最大位置序列，，，，，，和分别为单位矩阵和零矩阵，为时间步长参数，K为预测视界长度，，为智能体i到达t _z时刻的第k _t预测时间步的初始状态。

7.根据权利要求1所述的一种集群智能系统协同运动规划与编队控制方法，其特征在于，步骤S6中带有软约束的按需避碰策略具体为：

其中，，为对角矩阵，和分别为智能体i和智能体j利用第预测时间步的可用信息得到的第预测时间步的预测位置信息，为智能体i利用第预测时间步的可用信息得到的第预测时间步的预测位置信息，n为模拟边界的阶数，表示在第预测时间步上检测到发生碰撞的预测视界，，为2范数，为智能体间的安全距离，为决策变量。

8.根据权利要求1或7所述的一种集群智能系统协同运动规划与编队控制方法，其特征在于，步骤S6中采用带有软约束的按需避碰策略对编队控制目标函数的优化过程进行迭代求解具体为：

其中，为代价函数，，为编队控制目标函数，，为所有决策变量的堆叠向量，，为标量优化参数，分别为零矩阵、全一矩阵和单位矩阵，T为矩阵转置符号，和分别为智能体i在初始时刻t ₀的位置信息和智能体i在预测视界为1的位置信息预测值，为智能体i设定的初始位置，和分别为智能体i利用第k _t预测时间步的可用信息得到的第预测时间步的状态预测值和加速度预测值，表示在第预测时间步上的预测视界，为智能体i利用第k _t预测时间步的可用信息得到的第预测时间步的状态预测值，，，，，，和分别为单位矩阵和零矩阵，为时间步长参数，K为预测视界长度，为智能体i达到t _z时刻的第k _t预测时间步的预测输入序列，，分别为最小输入序列和最大输入序列，，分别为运动空间的最小位置序列和最大位置序列，，为在k _t时刻的初始状态，k _t为预测时间步，，，，为智能体i到达t _z时刻的第k _t预测时间步的初始状态，为对角矩阵，n为模拟边界的阶数，和分别为智能体i和智能体j利用第预测时间步的可用信息得到的第预测时间步的预测位置信息，，为2范数，为智能体间的安全距离，为决策变量，和分别为智能体i在t _z时刻和t _z+1时刻的位置信息，为智能体j在t _z时刻的位置信息，h _i和h _j分别为智能体i和智能体j的期望编队构型常数，为智能体i的相邻智能体的集合，为智能体i的相邻智能体的个数。