基于距离的智能体分布式编队轨迹优化方法

1.本发明涉及一种基于距离的智能体分布式编队轨迹优化方法，属于智能体集群编队控制技术领域。


背景技术：

2.分布式编队队形控制是多智能体组成的编队集群，在面对不同环境和任务，整个集群编队能够自发自组织编队和控制，而且保证智能体间不发生碰撞，在一定规则或框架下，实现编队队形之间的平滑过渡。分布式协同编队控制主要解决以下三个问题：一是随机分布环境中的智能体编队重构问题，即未构成编队队形时的编队构成问题，或者编队队形遇到变形时维持队形结构问题；二是编队队形之间的切换问题，在面对不同场景下，调整编队队形问题。三是编队重构或者变换时，编队间碰撞问题。通过设计分布式编队控制算法解决上述三个问题。
3.根据控制方式的不同，多智能体编队控制主要可以分为两类：基于相对位置的编队控制和基于相对距离的编队控制。基于相对位置的编队是指期望的编队由期望的相对位置向量来确定，而基于相对距离的编队是指期望的编队由期望的相对距离来确定。前者期望的编队仅有单平衡点，可以由线性反馈控制策略来全局镇定。但该类控制方法不能避免自主体之间的相互碰撞。与前者不同的是，基于距离的编队控制能够避免自主体之间的碰撞问题。对于多智能体，在各种情况下，以规定的队形导航被广泛应用。然而，相关的规划策略通常缺乏在杂乱环境中避免障碍的能力。


技术实现要素：

4.为解决上述技术问题，本发明提出一种在复杂环境下保持编队队形的多智能体轨迹规划方法。基于智能体动力学、平滑度和安全性，设计了一种考虑时间的分散式轨迹规划，将该轨迹表示法放入多智能体场景中，提高了编队运动的实用性和安全性。本发明的具体技术方案如下：
5.一种保持编队队形的多智能体轨迹规划方法，包括以下步骤：
6.s1：建立系统数学模型，基于负梯度控制算法，提出了一种编队控制律。
7.s2：设计了基于图论的成本函数，量化了多智能体三维编队各条边距离权重的相似性。
8.s3：结合上述编队控制方法和轨迹优化的优点，提出了一种适合多智能体编队的轨迹规划方法。
9.进一步地，所述步骤s1具体为：
10.s1-1：对于平面中具有三个自主体的编队控制系统，每个自主体的动态用单积分器来表示：
[0011][0012]
其中ri＝[r
xi
,r
yi
]
t
表示自主体i的位置信息，vi＝[v
xi
,v
yi
]
t
表示自主体i的速度输
入，i＝1,2,3；令r
ij
＝r
i-rj表示自主体i和j的相对位置,j＝1,2,3且j≠i。
[0013]
s1-2：令a
ij
＝||r
ij
||2表示通信智能体之间的实际距离，d
ij
表示智能体i和j间期望距离。本发明中智能体i和j，j∈ni之间的势能函数表达式如下式所示：
[0014][0015]
其中，v
ij
表示智能体i和j之间的势能，vi表示智能体i的总势能；显然，当智能体i与其所有邻居之间的距离均达到期望值时，vi取最小值为零；当智能体i与其任意一个邻居之间的距离为零时，vi趋于无穷。此外，定义如下等式：
[0016][0017]
s1-3：为了达到期望的全局稳定编队，基于相对位置信息，提出一种编队控制策略。控制率vi如下式所示：
[0018][0019][0020][0021]
其中，表示智能体i的总势能梯度，中间变量
[0022]
进一步地，所述步骤s2的具体过程为：
[0023]
s2-1：n个智能体的编队由无向图g＝(v,e)，其中v＝{1,2,...,n}是顶点集，是边集。在图g中，顶点i表示第i个智能体，位置向量连接顶点i∈v和顶点j∈v的边e
ij
∈e表示智能体i和j可以测量彼此之间的几何距离。在本发明中，每个智能体都与其他所有智能体通信，因此编队图g是完整的。图g的每条边与权重的非负数相关。边e
ij
的权重由下式给出：
[0024]wij
＝||p
i-pj||2,(i,j)∈ε
[0025]
其中||
·
||表示欧几里得范数，表示智能体间的距离。
[0026]
s2-2：所以图g的邻接矩阵和度矩阵已经确定，那么相应的拉普拉斯矩阵由下式给出：
[0027]
l＝d-a
[0028]
利用上述矩阵，图g的对称归一化拉普拉斯矩阵定义为：
[0029][0030]
其中是单位矩阵。
[0031]
s2-3：作为图表示矩阵，拉普拉斯矩阵包含关于图结构的信息。为了实现期望的编队集群，我们采用一种编队相似距离度量。具体是采用编队相似距离度量f量化多智能体三维编队各条边距离权重的相似性：
[0032][0033]
其中是当前群编队形成的对称归一化拉普拉斯矩阵，二范数||
·
||用于表示距离度量，表示期望编队的拉普拉斯矩阵。
[0034]
进一步地，所述步骤s3的具体过程为：
[0035]
s3-1：路线图是环境ge＝(ve,ee)的无向连接图，其中每个顶点v∈ve对应于自由配置空间f中的位置，每条边(u,v)∈ee表示f中有一条连接u和v的线性路径。我们还要求存在一个顶点对应于每个起始位置si，并且对于每个目标位置gi存在顶点令是返回每个顶点位置的函数。具体步骤为：
[0036]
(1)构建连通图：在空间中随机抽样，采集一系列点，并检测它们是否在安全空间中，根据一定的步长将这些采样点相互连接起来，仅保留与障碍物无交集的连线，结合起始点和目标点，得到一张连通图；
[0037]
(2)搜索最佳路径：使用图搜索算法基于上述连通图搜索无碰撞路径，如果能找到起始点到终点的路线，说明存在可行的运动规划方案；
[0038]
(3)设定路线图约束，获得路线图。
[0039]
通常生成路线图来规划单个机器人的运动。如果多个机器人使用相同的路线图，则会有额外的约束，如下所示。
[0040]
(1)点对点约束：两个智能体可能不会同时占据彼此非常接近的两个顶点。
[0041]
(2)边对边约束：如果在移动过程中可能发生碰撞，两个智能体无法移动两条边。
[0042]
(3)边对点约束：防止一个智能体可能与另一个静止智能体发生碰撞。
[0043]
s3-2：我们得到了一个环境路线图，其中包含上一步骤中生成的额外冲突集。在每个时间间隔，智能体可以在当前顶点等待，也可以遍历边。每个智能体的离散调度pi是由k 1个位置序列组成：
[0044][0045]
智能体应在时间上同步，以便智能体i在间隔k处位于路径点在路径点和之间，假设智能体在和之间的线段上运动，将该线段表示为
[0046]
s3-3：在连续细化阶段，本发明将离散规划器生成的路径点序列pi转换为平滑轨迹fi。使用离散规划来划分自由空间f，以便每个智能体在保证无碰撞的区域内解决独立的平滑轨迹优化问题。我们将时间tk＝kδt分配给每个离散时间步长，其中δt是用户指定的参数。这是对t＝kδt的初步猜测。精确的总时间t在后处理阶段计算，使得所有轨迹满足给定的物理限制。具体步骤为：
[0047]
将离散调度pi转换为平滑轨迹fi：
[0048]
其中，k表示曲线阶数，t∈[0,1]且从0开始以10-k
步长递增到1；轨迹fi包含起始点、终止点、控制点；其中起始点和终止点分别为p0和pk，控制点由上述公式中t的递增结合pi的中间点
会不断生成；最终将密集的控制点连接起来，得到一条平滑的轨迹曲线。
[0049]
本发明的有益效果
[0050]
1.本发明提出了一种用于密集环境下飞行的分布式编队轨迹优化方法。提出了一种评价当前编队智能体间实际距离与期望距离相似度的方法，在基于距离的编队控制方法上，使用三个阶段创建轨迹：带有智能体间冲突注释的路线图生成、离散规划和连续优化。路线图生成阶段为单智能体创建一个稀疏的路线图，并且可以使用任何现有算法来实现。然后用一般化的冲突来注释路线图，输出是一个离散的时间表，如果执行该时间表，将需要智能体频繁停止。连续优化可以为每个智能体找到平滑的轨迹，从而便于并行化，提高智能体集群的性能。
[0051]
2.本发明从实际出发，根据编队控制问题考虑了避障、编队相似性、动力学实际性等多种约束，提升了编队控制的安全性和实用性，为多智能体在编队基础上实现进一步协作提供了可能。
附图说明
[0052]
图1是本发明的方法流程图；
[0053]
图2是本发明的仿真实验图；
具体实施方式
[0054]
为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本发明的实施例及实施例中的特征可以相互组合。
[0055]
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。
[0056]
本发明的技术方案为：首先，建立系统数学模型，基于负梯度控制算法设计多智能体编队控制方法；其次，设计了基于图论的成本函数，量化了多智能体三维编队各条边距离权重的相似性；最后，结合基于上述编队控制方法和轨迹优化的优点，提出了一种适合多智能体编队的轨迹规划方法。
[0057]
具体地，如图1所示，一种保持编队队形的多智能体轨迹规划方法，包括以下步骤：
[0058]
s1：建立系统数学模型，基于负梯度控制算法，提出了一种编队控制律。
[0059]
s2：设计了基于图论的成本函数，量化了多智能体三维编队各条边距离权重的相似性。
[0060]
s3：结合上述编队控制方法和轨迹优化的优点，提出了一种适合多智能体编队的轨迹规划方法。
[0061]
进一步地，所述步骤s1具体为：
[0062]
s1-1：对于平面中具有三个自主体的编队控制系统，每个自主体的动态用单积分器来表示：
[0063][0064]
其中ri＝[r
xi
,r
yi
]
t
表示自主体i的位置信息，vi＝[v
xi
,v
yi
]
t
表示自主体i的速度输
入，i＝1,2,3；令r
ij
＝r
i-rj表示自主体i和j的相对位置,j＝1,2,3且j≠i。
[0065]
s1-2：令a
ij
＝||r
ij
||2表示通信智能体之间的实际距离，d
ij
表示智能体i和j间期望距离。本发明中智能体i和j，j∈ni之间的势能函数表达式如下式所示：
[0066][0067]
其中，v
ij
表示智能体i和j之间的势能，vi表示智能体i的总势能；显然，当智能体i与其所有邻居之间的距离均达到期望值时，vi取最小值为零；当智能体i与其任意一个邻居之间的距离为零时，vi趋于无穷。此外，定义如下等式：
[0068][0069]
s1-3：为了达到期望的全局稳定编队，基于相对位置信息，提出一种编队控制策略。控制率vi如下式所示：
[0070][0071][0072][0073]
其中，
▽
rivi
表示智能体i的总势能梯度，中间变量
[0074]
进一步地，所述步骤s2的具体过程为：
[0075]
s2-1：n个智能体的编队由无向图g＝(v,e)，其中v＝{1,2,...,n}是顶点集，是边集。在图g中，顶点i表示第i个智能体，位置向量连接顶点i∈v和顶点j∈v的边e
ij
∈e表示智能体i和j可以测量彼此之间的几何距离。在本发明中，每个智能体都与其他所有智能体通信，因此编队图g是完整的。图g的每条边与权重的非负数相关。边e
ij
的权重由下式给出：
[0076]wij
＝||p
i-pj||2,(i,j)∈ε
[0077]
其中||
·
||表示欧几里得范数，表示智能体间的距离。
[0078]
s2-2：所以图g的邻接矩阵和度矩阵已经确定，那么相应的拉普拉斯矩阵由下式给出：
[0079]
l＝d-a
[0080]
利用上述矩阵，图g的对称归一化拉普拉斯矩阵定义为：
[0081][0082]
其中是单位矩阵。
[0083]
s2-3：作为图表示矩阵，拉普拉斯矩阵包含关于图结构的信息。为了实现期望的编队集群，我们采用一种编队相似距离度量。具体是采用编队相似距离度量f量化多智能体三维编队各条边距离权重的相似性：
[0084][0085]
其中是当前群编队形成的对称归一化拉普拉斯矩阵，二范数||
·
||用于表示距离度量，表示期望编队的拉普拉斯矩阵。
[0086]
进一步地，所述步骤s3的具体过程为：
[0087]
s3-1：路线图是环境ge＝(ve,ee)的无向连接图，其中每个顶点v∈ve对应于自由配置空间f中的位置，每条边(u,v)∈ee表示f中有一条连接u和v的线性路径。我们还要求存在一个顶点对应于每个起始位置si，并且对于每个目标位置gi存在顶点令是返回每个顶点位置的函数。具体步骤为：
[0088]
(1)构建连通图：在空间中随机抽样，采集一系列点，并检测它们是否在安全空间中，根据一定的步长将这些采样点相互连接起来，仅保留与障碍物无交集的连线，结合起始点和目标点，得到一张连通图；
[0089]
(2)搜索最佳路径：使用图搜索算法基于上述连通图搜索无碰撞路径，如果能找到起始点到终点的路线，说明存在可行的运动规划方案；
[0090]
(3)设定路线图约束，获得路线图。
[0091]
通常生成路线图来规划单个机器人的运动。如果多个机器人使用相同的路线图，则会有额外的约束，如下所示。
[0092]
(1)点对点约束：两个智能体可能不会同时占据彼此非常接近的两个顶点。
[0093]
(2)边对边约束：如果在移动过程中可能发生碰撞，则两个智能体无法移动两条边。
[0094]
(3)边对点约束：防止一个智能体可能与另一个静止智能体发生碰撞。
[0095]
s3-2：我们得到了一个环境路线图，其中包含上一步骤中生成的额外冲突集。在每个时间间隔，智能体可以在当前顶点等待，也可以遍历边。每个智能体的离散调度pi是由k 1个位置序列组成：
[0096][0097]
智能体应在时间上同步，以便智能体i在间隔k处位于路径点在路径点和之间，假设智能体在和之间的线段上运动，将该线段表示为
[0098]
s3-3：在连续细化阶段，本发明将离散规划器生成的路径点序列pi转换为平滑轨迹fi。使用离散规划来划分自由空间f，以便每个智能体在保证无碰撞的区域内解决独立的平滑轨迹优化问题。我们将时间tk＝kδt分配给每个离散时间步长，其中δt是用户指定的参数。这是对t＝kδt的初步猜测。精确的总时间t在后处理阶段计算，使得所有轨迹满足给定的物理限制。具体步骤为：
[0099]
将离散调度pi转换为平滑轨迹fi：
[0100][0101]
其中，k表示曲线阶数，t∈[0,1]且从0开始以10-k
步长递增到1；轨迹fi包含起始
点、终止点、控制点；其中起始点和终止点分别为p0和pk，控制点由上述公式中t的递增结合pi的中间点会不断生成；最终将密集的控制点连接起来，得到一条平滑的轨迹曲线。
[0102]
为验证本发明的一种保持编队队形的多智能体轨迹规划方法的有效性，进行了仿真与实际飞行验证。下面结合实验和附图对本发明基于距离和轨迹优化算法的多智能体编队控制方法做出详细说明。
[0103]
一、实验平台简介
[0104]
该平台由三架轴距0.6m的四旋翼无人机组成，无人机均搭载一块英伟达nx嵌入式计算机用于编队轨迹优化的控制算法实现，gps模块提供无人机的定位数据，搭配rtk模块进一步提高无人机定位精确度。地面端和机载端均运行linux操作系统执行代码和处理用户指令，并将生成的虚拟轨迹发送给目标无人机。
[0105]
二、编队控制实验
[0106]
实验平台由三架轴距0.6m的四旋翼无人机组成，无人机均搭载一块英伟达nx嵌入式计算机用于编队轨迹优化的控制算法实现，gps模块提供无人机的定位数据，搭配rtk模块进一步提高无人机定位精确度。地面端和机载端均运行linux操作系统执行代码和处理用户指令，并将生成的虚拟轨迹发送给目标无人机。
[0107]
二、编队控制实验
[0108]
多智能体仿真实验由matlab平台实现，仿真结果如图2所示。该实验设计了四智能体保持三角锥队形螺旋上升运动，图2描述了各智能体在同一坐标系下的实时位置曲线，可以看出编队在各个时段实现稳定的编队保持。实物实验采用了三架搭载板载计算机的四旋翼无人机，实现了三架无人机三角形编队的稳定飞行。
[0109]
经过上述分析，证明了本发明所提算法的有效性。
[0110]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。