CN115033016A - 一种异构无人集群编队避障方法及系统 - Google Patents

Abstract

本发明公开了一种异构无人集群编队避障方法及系统。该方法中根据异构无人集群编队避障系统确定领航者和跟随者，采用领航者可切换的方式，在集群中找到最合适的智能体作为领航者；根据异构无人集群编队避障系统的运动状态以及环境中障碍物的位置构造分布式的编队避障控制器；根据异构无人集群编队避障系统的运动状态以及控制输入构造运动学约束模块保证智能体的控制输入限制在相应智能体的运动学约束范围之内；根据异构无人集群编队避障系统的通信拓扑关系构造通信拓扑模块；根据异构无人集群编队避障系统中智能体的异构性构造空地协同模块；本发明实现存在障碍物条件下的编队和避障之间的权衡控制。

Description

一种异构无人集群编队避障方法及系统

技术领域

本发明涉及异构多智能体协同控制领域，特别是涉及一种异构无人集群编队避障方法及系统。

背景技术

在过去几十年中，由于无人机、无人车在军事和民用领域的巨大潜在应用，多智能体领域得到了快速发展。目前，学者们对于多智能体的研究主要集中在同构多智能体上，对于异构多智能体的研究还十分有限，但是在实际的应用场景下，同构智能体有时无法满足人们的要求，往往需要多种异构的智能体协同起来共同完成某项复杂的任务。例如，在对未知区域探索的场景下，无人机由于机动性强、受环境条件限制小等因素会进入狭小的区域开展任务，但是由于其电量、算力资源有限，无法长时间的进行探索；而无人车可以携带充足的算力对未知场景进行精细的探索，但是无人车由于体积大等因素无法进入狭小的空间进行探索；然而无人机与无人车的结合，可以以更高的效率完成更复杂的任务。因此，亟需研究异构无人系统的协同控制问题。

异构多智能体在执行任务时往往需要以一定的队形进行，异构多智能体的编队具有很多的应用场景，能够以更高的鲁棒性完成探索、覆盖等复杂任务。针对无人系统的编队问题已有大量的研究工作，然而现有的无人集群编队系统相对简单，一般假设集群系统是同构的。在实践中，环境中会出现大量障碍物，为了安全起见，智能体需要躲避障碍物并躲避邻居机器人防止发生碰撞。目前异构集群编队避障控制的研究还不成熟，智能体在避障及避碰时会偏移出编队期望队形，导致编队队形无法继续保持。如何系统地权衡编队及避障这两个相互冲突的要求，是实现无碰撞编队行驶的关键。

人工势场法和动态窗口法是智能体避障、避碰传统有效的方法。其中人工势场是指将智能体所在的环境当做一个虚拟的场，目标点可以对智能体产生引力场，障碍物对智能体产生斥力场，智能体在引力场和斥力场相互作用下到达目的地、形成期望的编队。但是人工势场法存在局部最优值问题，现有解决局部最优值的方法是对智能体施加一个随机的扰动，使得智能体跳出局部最优点，但是随机的扰动可能导致智能体无法保持期望的编队队形，导致编队队形被破坏。动态窗口法是指考虑机器人的朝向、速度、到障碍物的距离等因素的条件下，在可行的速度空间内选择最优的速度，以最优的轨迹到达指定目标点。在编队避障系统中采用动态窗口法需要引入多个虚拟节点，导致智能体的通信拓扑变得复杂，更适用于单个智能体的避障。综上所述，研究异构无人集群编队避障系统不仅具有理论意义，更加具有工程实际意义。

发明内容

本发明主要是解决现有技术所存在的缺乏合适的异构无人集群编队避障方法的技术问题，提供一种可选择编队领航者且在智能体避障过程中可以保持编队队形的异构无人集群编队避障方法及系统。

本发明针对上述技术问题主要是通过下述技术方案得以解决的：

一种异构无人集群编队避障方法，包括以下步骤：

S101、获取异构无人集群编队避障系统的通信拓扑关系，确定编队系统的稳定性；所述的异构无人集群编队避障的通信拓扑关系包括无人车之间的通信拓扑图和无人机之间的通信拓扑图，智能体只能与同构的邻居智能体进行通信，且智能体与智能体之间的作用拓扑是双向的；

S102、根据异构无人集群编队避障系统中智能体的运动状态和控制输入，确定智能体的运动学模型；

S103、根据智能体的运动状态和障碍物的位置信息，选择无人车领航者和无人机领航者；

S104、构造分布式的编队形成控制器，确定在无障碍物环境下的控制输入，所述的编队形成控制器生成所述的异构无人集群编队避障系统在无障碍物环境下整体运动的参考轨迹；

S105、根据智能体的位置信息，确定编队避障系统的权重向量，并利用权重向量优化无障碍物环境下整体运动的参考轨迹；

S106、构造分布式的编队避障结合控制器，确定在有障碍物环境下控制输入，对异构无人集群编队避障系统在有障碍物的环境下实现编队和避障的权衡与结合；

S107、构造运动学约束模块，限制智能体的控制输入在智能体的约束范围之内。

作为优选，步骤S101具体包括：

同构无人集群编队(包括无人机编队和无人车编队)的通信拓扑用图

表示，

表示节点集合，

表示边集合，

表示第i个智能体与第j个智能体的通信关系；令ε_ij＝(v_i,v_j)表示图

中从节点v_i到节点v_j的一条边；当且仅当第i个智能体与第j个智能体互为邻居节点，即第i个智能体与第j个智能体之间可以进行通信时，令ω_ij＝1，否则ω_ij＝0；用

表示节点v_i的邻居节点集合；将图

的邻接矩阵定义为

将图

的入度矩阵定义为

其中，

定义图的拉普拉斯矩阵为

对图的动力系统进行分析，定义矩阵

的约当标准形式为

其中，矩阵

式中λ_i为矩阵

的特征值，V_i为矩阵

的右特征向量，满足

其中

为单位矩阵；矩阵

的左特征向量ω_i满足

矩阵

的任意一行元素之和都为0，所以当且仅当

时，特征值为0，其中c为唯一特征向量，此时

的秩为N-1，从而使得拓扑图

可以有一个生成树，系统是可以稳定的。

作为优选，步骤S102具体包括：

首先利用公式

确定无人车的运动模型；

利用公式

确定无人机的运动模型；

其中，x_i-UGV(t)∈R^2×1是第i个无人车节点的位置信息，

是x_i-UGV(t)的微分，u_i-UGV(t)∈R^2×1为第i个无人车节点的控制输入量，v_i-UGV(t)∈R^2×1为第i个无人车节点的速度信息，x_i-UAV(t)∈R^3×1是第i个无人机节点的位置信息，

是x_i-UAV(t)的微分，u_i-UAV(t)∈R^3×1为第i个无人机节点的控制输入量，v_i-UAV(t)∈R^3×1为第i个无人机节点的速度信息。

作为优选，步骤S103具体包括：

初始时刻根据通信拓扑图

随机选择智能体作为所述的无人集群编队避障系统的无人车领航者和无人机领航者；

根据所述的异构无人集群编队避障系统的运动状态以及环境中障碍物的位置，利用公式

得到可以作为领航者的智能体集合，判断所述的随机选择的无人车领航者和无人机领航者是否在可以作为领航者的智能体集合L_i中；

若所述的随机选择的无人车领航者或无人机领航者不在所述的可以作为领航者的智能体的集合L_i中，则认为当前随机选择的无人车领航者或无人机领航者无效，那么重新在所述的可以作为领航者的智能体集合L_i中随机选择异构无人集群编队避障系统的无人车领航者或无人机领航者；若所述的随机选择的无人车领航者和无人机领航者在所述的可以作为领航者的智能体的集合L_i中，则认为当前随机选择的无人车领航者和无人机领航者有效；

其中，ε为智能体需要避障时到障碍物的最小距离，x_obs-UGV∈R^2×1表示二维空间上障碍物的位置信息，x_obs-UAV∈R^3×1表示三维空间上障碍物的位置信息，N为同构智能体的数量。如果无人机的数量和无人车的数量不一致，则对N进行适应性调整即可。初始时刻随机选择智能体是因为在系统初始时刻智能体运行不稳定，运行稳定之后才能判断障碍物的距离，然后在待选集合中选择领航者。

作为优选，步骤S104具体包括：

利用公式v_L-UGV(t)＝u_L-UGV(t)＝KN*(x_goal-UGV-x_L-UGV(t))，确定在没有障碍物环境下的无人车领航者的控制输入；

利用公式v_L-UAV(t)＝u_L-UAV(t)＝KN*(x_goal-UAV-x_L-UAV(t))，确定在没有障碍物环境下的无人机领航者的控制输入；

令x_goal-UAV＝[x_L-UGV(t)；H]，无人机领航者的目标位置信息即为无人车领航者的位置信息对无人机期望的飞行高度H的增广，实现无人机领航者对无人车领航者的跟踪，保证空地智能体协同进行编队；

其中，H表示无人机期望的飞行高度；

利用公式

确定在没有障碍物环境下的无人车跟随者的控制输入；

利用公式

确定在没有障碍物环境下的无人机跟随者的控制输入；

其中，v_L-UGV(t)∈R^2×1是无人车领航者的速度信息，u_L-UGV(t)∈R^2×1是无人车领航者的控制输入量，KN表示领航者跟踪目标增益系数，x_goal-UGV∈R^2×1表示无人车领航者的目标点，x_L-UGV(t)∈R^2×1是无人车领航者的位置信息，v_L-UAV(t)∈R^3×1是无人机领航者的速度信息，u_L-UAV(t)∈R^3×1是无人机领航者的控制输入量，x_goal-UAV∈R^3×1表示无人机领航者的目标点，x_L-UAV(t)∈R^3×1是无人机领航者的位置信息，v_i-F-UGV(t)∈R^2×1是第i个无人车跟随者的速度信息，u_i-F-UGV(t)∈R^2×1表示第i个无人车跟随者的控制输入，K0表示一致性增益系数，ω_ij表示权重，N_i表示与第i个智能体互为邻居的智能体的集合，x_i-F-UGV(t)∈R^2×1表示第i个无人车跟随者的位置信息，x_j-UGV(t)∈R^2×1表示第j个无人车的位置信息，

表示第i个无人车跟随者的期望位置，

表示第j个无人车的期望位置，v_i-F-UAV(t)∈R^3×1是第i个无人机跟随者的速度信息，u_i-F-UAV(t)∈R^3×1表示第i个无人机跟随者的控制输入，x_i-F-UAV(t)∈R^3×1表示第i个无人机跟随者的位置信息，x_j-UAV(t)∈R^3×1表示第j个无人机的位置信息，

表示第i个无人机跟随者的期望位置，

表示第j个无人机的期望位置；

S104具体还包括：

利用公式

其中j∈N_i，得到第i个无人车与其邻居智能体形成期望编队的误差；

利用公式

其中j∈N_i，得到第i个无人机与其邻居智能体形成期望编队的误差；

利用公式distance_UGV_i(t)＝(sum_delta_UGV_F_i(t)^T·sum_delta_UGV_F_i(t))^1/2，得到第i个无人车与其邻居智能体形成期望编队的误差距离；

利用公式distance_UAV_i(t)＝(sum_delta_UAV_F_i(t)^T·sum_delta_UAV_F_i(t))^1/2，得到第i个无人机与其邻居智能体形成期望编队的误差距离；

利用公式v_i-F-UGV(t)＝u_i-F-UGV(t)＝K0·v_L-UGV(t)+gama·distance_UGV_i(t)，得到在没有障碍物环境下的无人车跟随者的控制输入；

利用公式v_i-F-UAV(t)＝u_i-F-UAV(t)＝K0·v_L-UAV(t)+gama·distance_UAV_i(t)，得到在没有障碍物环境下的无人机跟随者的控制输入；

其中，sum_delta_UGV_F_i(t)∈R^2×1表示第i个无人车与其邻居智能体形成期望编队的误差，A(i,j)表示第i个无人车和第j个无人车的通信拓扑关系，当第i个无人车和第j个无人车存在通信时A(i,j)＝1，否则A(i,j)＝0，sum_delta_UAV_F_i(t)∈R^3×1表示第i个无人机与其邻居智能体形成期望编队的误差，B(i,j)表示第i个无人机和第j个无人机的通信拓扑关系，当第i个无人机和第j个无人机存在通信时B(i,j)＝1，否则B(i,j)＝0，distance_UGV_i(t)∈R表示第i个无人车与其邻居智能体形成期望编队的误差距离，distance_UAV_i(t)∈R表示第i个无人机与其邻居智能体形成期望编队的误差距离，gama为跟随者跟踪目标增益系数。

作为优选，步骤S105具体包括：

利用公式ω_UGV_ij＝2-exp(-sum_delta_UGV_F_i(t)^T·sum_delta_UGV_F_i(t))，得到第i个无人车用于编队形成控制的权重；

利用公式ω_UAV_ij＝2-exp(-sum_delta_UAV_F_i(t)^T·sum_delta_UAV_F_i(t))，得到第i个无人机用于编队形成控制的权重；

其中，ω_UGV_ij∈R为第i个无人车用于编队形成控制的权重，ω_UAV_ij∈R为第i个无人机用于编队形成控制的权重；

将S105中的权重向量用在S104的编队形成控制器中，生成在无障碍物环境下的编队整体运动的优化参考轨迹；

利用公式

其中j∈N_i，得到第i个无人车与其邻居智能体形成期望编队的误差最小值；

利用公式

其中j∈N_i，得到第i个无人机与其邻居智能体形成期望编队的误差最小值；

利用公式distance_UGV_i(t)＝(sum_delta_UGV_F_i(t)^T·sum_delta_UGV_F_i(t))^1/2，得到第i个无人车与其邻居智能体形成期望编队的最小误差距离之和；

利用公式distance_UAV_i(t)＝(sum_delta_UAV_F_i(t)^T·sum_delta_UAV_F_i(t))^1/2，得到第i个无人机与其邻居智能体形成期望编队的最小误差距离之和；

利用公式v_i-F-UGV(t)＝u_i-F-UGV(t)＝K0·v_L-UGV(t)+gama·distance_UGV_i(t)，得到在没有障碍物环境下的无人车跟随者的控制输入；

利用公式v_i-F-UAV(t)＝u_i-F-UAV(t)＝K0·v_L-UAV(t)+gama·distance_UAV_i(t)，得到在没有障碍物环境下的无人机跟随者的控制输入。

作为优选，步骤S106具体包括：

结合S104所述的构造分布式编队形成控制器和S105所述的确定编队避障系统的权重向量，在有障碍物的环境下：

利用公式：u_L-UGV-ob(t)＝v_L-UGV-ob(t)＝v_L-UGV(t)+σ(β·ob_dist(v,w)+γ·velocity(v,w))，确定在有障碍物环境下的无人车领航者的控制输入；

利用公式：u_L-UAV-ob(t)＝v_L-UAV-ob(t)＝v_L-UAV(t)+σ(β·ob_dist(v,w)+γ·velocity(v,w))，确定在有障碍物环境下的无人机领航者的控制输入；

其中v_L-UGV-ob(t)∈R^2×1、v_L-UAV-ob(t)∈R^3×1分别表示无人车领航者和无人机领航者的速度信息，ob_dist(v,w)表示智能体到障碍物的距离，velocity(v,w)表示智能体的可行速度，σ、β和γ表示动态窗口避障增益系数；

S106具体还包括：

判断所述的无人车跟随者的位置信息x_i-F-UGV(t)和所述无人机跟随者在二维平面XOY上的的位置信息x_i-F-UAV-XOY(t)，是否满足：

若满足，则说明所述的无人车跟随者和无人机跟随者ε领域内无障碍物，可按照S105中的控制输入进行编队控制；若不满足，则说明所述的无人车跟随者和无人机跟随者的ε领域内有障碍物，需要进行编队避障控制：

利用公式

确定在有障碍物环境下所述的无人车跟随者受到障碍物的斥力大小；

利用公式

确定在有障碍物环境下所述的无人机跟随者在XOY平面上受到障碍物的斥力大小；

利用不等式

确定所述的无人机跟随者是否受到障碍物在Z轴方向上的斥力；

若当前无人机跟随者的位置x_i-F-UAV(t)满足上述不等式，则利用公式

确定在Z轴方向上所述的无人机跟随者受到障碍物的斥力大小；若当前无人机跟随者的位置x_i-F-UAV(t)不满足上述不等式，那么所述的无人机跟随者只受到障碍物在XOY平面上的斥力，而不受到障碍物在Z轴方向上的斥力；

利用公式r_i-F-UAV-ob＝r_{i-F-UAV-ob-XOY}+r_i-F-UAV-ob-Z得到所述的无人机跟随者受到的障碍物的总斥力；

利用公式

计算障碍物产生的斥力系数；

其中，r_i-F-UGV-ob∈R^2×1表示无人车跟随者在障碍物作用下所受的斥力大小，N_obs-UGV∈R^2×1表示二维空间中的障碍物位置集合，temp表示障碍物产生的斥力系数，x_m-obs-UGV∈R^{2 ×1}表示二维空间上的第m个障碍物的位置信息，r_{i-F-UAV-ob-XOY}∈R^2×1表示无人机跟随者在二维平面XOY上所受到的斥力大小，N_obs-UAV∈R^3×1表示三维空间中的障碍物位置集合，x_i-F-UAV-XOY(t)∈R^2×1表示第i个无人机跟随者在XOY平面上的位置信息，x_{m-obs-UAV-XOY}∈R^2×1表示三维空间上的第m个障碍物在XOY平面上的位置信息，r_i-F-UAV-ob-Z∈R表示无人机跟随者在Z轴方向上所受到的斥力大小，x_i-F-UAV-Z(t)∈R表示第i个无人机跟随者在Z轴方向上的位置信息，x_m-obs-UAV-Z∈R表示三维空间上的第m个障碍物在Z轴方向上的位置信息，H∈R表示无人机期望的飞行高度，r_i-F-UAV-ob∈R^3×1表示第i个无人机跟随者在障碍物作用下所受的总斥力，ob_dist表示智能体到障碍物的距离，ε为智能体需要避障时到障碍物的最小距离；

S106具体还包括：

利用公式

得到无人车跟随者和无人机跟随者的控制输入，其中，beta为人工势场增益系数；

定义编队的总误差

利用公式

确定最优的人工势场增益系数beta，用于所述的异构无人集群编队避障系统的跟随者形成最优的参考轨迹；

其中，error_formation(beta)是所述的异构无人集群编队避障系统智能体编队队形的总误差，ob_dist(beta)是智能体到障碍物的距离，δ、

是编队误差权重增益系数；

S106具体还包括：

判断跟随者控制u_i-F-UGV-ob(t)、u_i-F-UAV-ob(t)是否为0；

若所述的跟随者的控制输入为0，判断所述的异构无人集群编队避障系统的跟随者是否形成期望的编队队形并到达期望的位置；若所述的跟随者未形成期望的编队队形或者未到达期望的位置，则利用公式

重新规划控制输入，用于所述的异构无人集群编队避障系统的跟随者跳出局部最小值点，重新生成参考轨迹；

其中，β是动态窗口避障增益系数。本方案中的各增益系数除特别说明以外均为通过试验得到的经验值。

作为优选，步骤S107具体包括：

利用公式confine_UGV＝‖v_i-UGV(k+1)‖_F-‖v_i-UGV(k)‖_F，得到无人车相邻两次规划的控制输入的差值，若confine_UGV>0，表示无人车规划的速度增大，若confine_UGV<0，表示无人车规划的速度减小；

考虑到无人车的最大加速度是有限制的，若confine_UGV>0，则利用公式v_i-UGV(k+1)＝min{v_i-UGV(k)+confine_UGV,v_i-UGV(k)+acc_UGV*dt}，若confine_UGV<0，则利用公式v_i-UGV(k+1)＝max{v_i-UGV(k)+confine_UGV,v_i-UGV(k)-acc_UGV*dt}；

考虑到无人车的最大速度是有限制的，若v_i-UGV(k+1)≥0，则利用公式v_i-UGV(k+1)＝min{v_i-UGV(k+1),v_max-UGV}，若v_i-UGV(k+1)<0，则利用公式v_i-UGV(k+1)＝max{v_i-UGV(k+1),-v_max-UGV}，从而将无人车的控制输入限制在其运动学约束范围之内；

其中，confine_UGV∈R表示无人车上一时刻的速度与当前时刻速度之差，v_i-UGV(k+1)∈R^2×1、v_i-UGV(k)∈R^2×1是将v_i-UGV(t)∈R^2×1离散化的结果，分别表示第i个无人车下一时刻的速度与当前时刻的速度，acc_UGV∈R表示无人车的最大加速度，dt表示无人车规划一次控制输入需要的时间，v_max-UGV∈R表示无人车的最大速度；

同理，利用公式confine_UAV＝‖v_i-UAV(k+1)‖_F-‖v_i-UAV(k)‖_F，得到无人机相邻两次规划的控制输入的差值，若confine_UAV>0，表示无人机规划的速度增大，若confine_UAV<0，表示无人机规划的速度减小；

考虑到无人机的最大加速度是有限制的，若confine_UAV>0，则利用公式v_i-UAV(k+1)＝min{v_i-UAV(k)+confine_UAV,v_i-UAV(k)+acc_UAV*dt}，若confine_UAV<0，则利用公式v_i-UAV(k+1)＝max{v_i-UAV(k)+confine_UAV,v_i-UAV(k)-acc_UAV*dt}；

考虑到无人机的最大速度是有限制的，若v_i-UAV(k+1)≥0，则利用公式v_i-UAV(k+1)＝min{v_i-UAV(k+1),v_max-UAV}，若v_i-UAV(k+1)<0，则利用公式v_i-UAV(k+1)＝max{v_i-UAV(k+1),-v_max-UAV}，从而将无人机的控制输入限制在其运动学约束范围之内；

其中，confine_UAV∈R表示无人机上一时刻的速度与当前时刻速度之差，v_i-UAV(k+1)∈R^3×1、v_i-UAV(k)∈R^3×1是将v_i-UAV(t)∈R^3×1离散化的结果，分别表示第i个无人机下一时刻的速度与当前时刻的速度，acc_UAV∈R表示无人机的最大加速度，dt表示无人机规划一次控制输入需要的时间，v_max-UAV∈R表示无人机的最大速度。

一种异构无人集群编队避障系统，运行有前述的异构无人编队避障方法，包括：

选择领航者模块701，用于异构无人集群编队避障系统确定无人车领航者和无人机领航者，所述的无人车领航者和无人机领航者分别是异构无人集群编队避障系统中具有异构性的不同类型的智能体；

通信拓扑模块702，用于构建异构无人集群编队避障系统的通信拓扑关系，所述的异构无人集群编队避障系统中的智能体只能与同构的邻居智能体进行通信，且智能体与智能体之间的作用拓扑是双向的；

编队避障模块703，用于生成所述的异构无人集群编队避障系统编队的参考轨迹；所述的编队避障模块具体包括编队形成模块和编队避障结合模块，其中，编队形成模块用于所述的异构无人集群编队避障系统的编队队形的形成，编队避障结合模块用于所述的异构无人集群编队避障系统在避障的过程中保持编队队形；所述的编队避障模块包含升降模块，用于所述的异构无人集群编队避障系统中的无人机在不改变编队队形的情况下躲避障碍物，实现在垂直于地面的方向上避障；

空地协同模块704，用于保证无人车和无人机之间协同进行编队；

运动学约束模块705，用于保证智能体的控制输入限制在相应智能体的运动学约束范围之内。

本发明所提供的一种异构无人集群编队避障方法，采用领航者可切换的方式，在集群中找到最合适的智能体作为领航者；在编队形成控制器中引入权重向量，以编队的整体位置误差作为编队队形的评价指标，提高编队队形形成的准确性和效率；改进传统的编队避障方式，领航者使用改进的动态窗口法，实现对编队轨迹的优化，跟随者使用改进的势场法，并选择最优的斥力场增益系数，实现在避障过程中保持编队队形的同时以最优的轨迹到达目标位置；在势场法中加入动态窗口，当跟随者陷入局部最优点时可以在保持编队队形的情况下跳出局部最优点；在无人机的避障过程中设计升降模块，保证在不破坏编队队形的情况下在垂直于地面的方向上越过障碍物，实现了存在障碍物条件下的编队和避障之间的权衡控制。

附图说明

图1为本发明所提供的一种异构无人集群编队避障方法流程示意图；

图2和图3为同构智能体之间的通信拓扑，其中图2表示无人车之间的通信拓扑图，图3表示无人机之间的通信拓扑图；

图4和图5为智能体初始时刻和终止时刻的位置，其中图4表示智能体初始时刻的相对位置图，图5表示智能体终止时刻的相对位置图；

图6为智能体运动轨迹图；

图7-图11为智能体的速度变化曲线图，其中图7表示无人车在X轴方向上的速度曲线，图8表示无人车在Y轴方向上的速度曲线，图9表示无人机在X轴方向上的速度，图10表示无人机在Y轴方向上的速度，图11表示无人机在Z轴方向上的速度；

图12和图13为智能体编队避障的权重累加图，其中图12表示无人车编队避障的权重累加图，图13表示无人机编队避障的权重累加图；

图14为本发明提供的一种异构无人编队避障系统的结构示意图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

实施例：如图1所示，本发明所提供的一种异构无人集群编队避障方法，包括：

S101，获取异构无人集群编队避障系统的通信拓扑关系，确定编队系统的稳定性；所述的异构无人集群编队避障的通信拓扑关系包括无人车之间的通信拓扑图和无人机之间的通信拓扑图，智能体只能与同构的邻居智能体进行通信，且智能体与智能体之间的作用拓扑是双向的；

S101具体包括：

异构无人集群编队避障系统的通信拓扑可用图

表示，

表示节点集合，

表示边集合，

表示第i个智能体与第j个智能体的通信关系。令ε_ij＝(v_i,v_j)表示图

中从节点v_i到节点v_j的一条边。当且仅当第i个智能体与第j个智能体互为邻居节点，即第i个智能体与第j个智能体之间可以进行通信时，令ω_ij＝1，否则ω_ij＝0。用

表示节点v_i的邻居节点集合。将图

的邻接矩阵定义为

将图

的入度矩阵定义为

其中，

定义图的拉普拉斯矩阵为

对图的动力系统进行分析，定义矩阵

的约当标准形式为

其中，矩阵

式中λ_i为矩阵

的特征值，V_i为矩阵

的右特征向量，满足

其中

为单位矩阵。矩阵

的左特征向量ω_i满足

矩阵

的任意一行元素之和都为0，所以当且仅当

时，特征值为0，其中c为唯一特征向量，此时

的秩为N-1，从而使得拓扑图

可以有一个生成树，系统是可以稳定的。

S102，根据异构无人集群编队避障系统中智能体的运动状态和控制输入，确定智能体的运动学模型；

首先利用公式

确定无人车的运动模型；

利用公式

确定无人机的运动模型；

其中，x_i-UGV(t)∈R^2×1是第i个无人车节点的位置信息，u_i-UGV(t)∈R^2×1为第i个无人车节点的控制输入量，v_i-UGV(t)∈R^2×1为第i个无人车节点的速度信息，x_i-UAV(t)∈R^3×1是第i个无人机节点的位置信息，u_i-UAV(t)∈R^3×1为第i个无人机节点的控制输入量，v_i-UAV(t)∈R^{3 ×1}为第i个无人机节点的速度信息。

S103，根据智能体的运动状态和障碍物的位置信息，选择无人车领航者和无人机领航者；

初始时刻根据通信拓扑图

随机选择智能体作为所述的无人集群编队避障系统的无人车领航者和无人机领航者；

根据所述的异构无人集群编队避障系统的运动状态以及环境中障碍物的位置，利用公式

得到可以作为领航者的智能体集合，判断所述的随机选择的无人车领航者和无人机领航者是否在可以作为领航者的智能体集合L_i中；

若所述的随机选择的无人车领航者或无人机领航者不在所述的可以作为领航者的智能体的集合L_i中，则认为当前随机选择的无人车领航者或无人机领航者无效，那么重新在所述的可以作为领航者的智能体集合L_i中随机选择异构无人集群编队避障系统的无人车领航者或无人机领航者；若所述的随机选择的无人车领航者和无人机领航者在所述的可以作为领航者的智能体的集合L_i中，则认为当前随机选择的无人车领航者和无人机领航者有效；

其中，ε为智能体需要避障时到障碍物的最小距离，x_obs-UGV∈R^2×1表示二维空间上障碍物的位置信息，x_obs-UAV∈R^3×1表示三维空间上障碍物的位置信息，N为同构智能体的数量；

S104，构造分布式的编队形成控制器，确定在无障碍物环境下的控制输入，所述的编队形成控制器用于生成所述的异构无人集群编队避障系统在无障碍物环境下整体运动的参考轨迹；

S104具体包括：

利用公式v_L-UGV(t)＝u_L-UGV(t)＝KN*(x_goal-UGV-x_L-UGV(t))，确定在没有障碍物环境下的无人车领航者的控制输入；

利用公式v_L-UAV(t)＝u_L-UAV(t)＝KN*(x_goal-UAV-x_L-UAV(t))，确定在没有障碍物环境下的无人机领航者的控制输入；

令x_goal-UAV＝[x_L-UGV(t)；H]，无人机领航者的目标位置信息即为无人车领航者的位置信息对无人机期望的飞行高度H的增广，实现无人机领航者对无人车领航者的跟踪，保证空地智能体协同进行编队；

其中，H表示无人机期望的飞行高度；

利用公式

确定在没有障碍物环境下的无人车跟随者的控制输入；

利用公式

确定在没有障碍物环境下的无人机跟随者的控制输入；

其中，v_L-UGV(t)∈R^2×1是无人车领航者的速度信息，u_L-UGV(t)∈R^2×1是无人车领航者的控制输入量，KN表示增益系数，x_goal-UGV∈R^2×1表示无人车领航者的目标点，x_L-UGV(t)∈R^2×1是无人车领航者的位置信息，v_L-UAV(t)∈R^3×1是无人机领航者的速度信息，u_L-UAV(t)∈R^3×1是无人机领航者的控制输入量，x_goal-UAV∈R^3×1表示无人机领航者的目标点，x_L-UAV(t)∈R^3×1是无人机领航者的位置信息，v_i-F-UGV(t)∈R^2×1是第i个无人车跟随者的速度信息，u_i-F-UGV(t)∈R^2×1表示第i个无人车跟随者的控制输入，K0表示增益系数，ω_ij表示权重，N_i表示与第i个智能体互为邻居的智能体的集合，x_i-F-UGV(t)∈R^2×1表示第i个无人车跟随者的位置信息，x_j-UGV(t)∈R^2×1表示第j个无人车的位置信息，

表示第i个无人车跟随者的期望位置，

表示第j个无人车的期望位置，v_i-F-UAV(t)∈R^3×1是第i个无人机跟随者的速度信息，u_i-F-UAV(t)∈R^3×1表示第i个无人机跟随者的控制输入，x_i-F-UAV(t)∈R^3×1表示第i个无人机跟随者的位置信息，x_j-UAV(t)∈R^3×1表示第j个无人机的位置信息，

表示第i个无人机跟随者的期望位置，

表示第j个无人机的期望位置；

S104具体还包括：

利用公式

其中j∈N_i，得到第i个无人车与其邻居智能体形成期望编队的误差；

利用公式

其中j∈N_i，得到第i个无人机与其邻居智能体形成期望编队的误差；

利用公式distance_UGV_i(t)＝(sum_delta_UGV_F_i(t)^T·sum_delta_UGV_F_i(t))^1/2，得到第i个无人车与其邻居智能体形成期望编队的误差距离；

利用公式distance_UAV_i(t)＝(sum_delta_UAV_F_i(t)^T·sum_delta_UAV_F_i(t))^1/2，得到第i个无人机与其邻居智能体形成期望编队的误差距离；

利用公式v_i-F-UGV(t)＝u_i-F-UGV(t)＝K0·v_L-UGV(t)+gama·distance_UGV_i(t)，得到在没有障碍物环境下的无人车跟随者的控制输入；

利用公式v_i-F-UAV(t)＝u_i-F-UAV(t)＝K0·v_L-UAV(t)+gama·distance_UAV_i(t)，得到在没有障碍物环境下的无人机跟随者的控制输入；

其中，sum_delta_UGV_F_i(t)∈R^2×1表示第i个无人车与其邻居智能体形成期望编队的误差，A(i,j)表示第i个无人车和第j个无人车的通信拓扑关系，当第i个无人车和第j个无人车存在通信时A(i,j)＝1，否则A(i,j)＝0，sum_delta_UAV_F_i(t)∈R^3×1表示第i个无人机与其邻居智能体形成期望编队的误差，B(i,j)表示第i个无人机和第j个无人机的通信拓扑关系，当第i个无人机和第j个无人机存在通信时B(i,j)＝1，否则B(i,j)＝0，distance_UGV_i(t)∈R表示第i个无人车与其邻居智能体形成期望编队的误差距离，distance_UAV_i(t)∈R表示第i个无人机与其邻居智能体形成期望编队的误差距离，gama为增益系数。

S105，根据智能体的位置信息，确定编队避障系统的权重向量，所述的权重向量用于提高编队队形形成的效率；

S105具体包括：

利用公式ω_UGV_ij＝2-exp(-sum_delta_UGV_F_i(t)^T·sum_delta_UGV_F_i(t))，得到第i个无人车用于编队形成控制的权重；

利用公式ω_UAV_ij＝2-exp(-sum_delta_UAV_F_i(t)^T·sum_delta_UAV_F_i(t))，得到第i个无人机用于编队形成控制的权重；

其中，ω_UGV_ij∈R为第i个无人车用于编队形成控制的权重，ω_UAV_ij∈R为第i个无人机用于编队形成控制的权重；

将S105中的权重向量用在S104的编队形成控制器中，用于生成在无障碍物环境下的编队整体运动的优化参考轨迹；

利用公式

其中j∈N_i，得到第i个无人车与其邻居智能体形成期望编队的误差最小值；

利用公式

其中j∈N_i，得到第i个无人机与其邻居智能体形成期望编队的误差最小值；

利用公式distance_UGV_i(t)＝(sum_delta_UGV_F_i(t)^T·sum_delta_UGV_F_i(t))^1/2，得到第i个无人车与其邻居智能体形成期望编队的最小误差距离之和；

利用公式distance_UAV_i(t)＝(sum_delta_UAV_F_i(t)^T·sum_delta_UAV_F_i(t))^1/2，得到第i个无人机与其邻居智能体形成期望编队的最小误差距离之和；

利用公式v_i-F-UGV(t)＝u_i-F-UGV(t)＝K0·v_L-UGV(t)+gama·distance_UGV_i(t)，得到在没有障碍物环境下的无人车跟随者的控制输入；

利用公式v_i-F-UAV(t)＝u_i-F-UAV(t)＝K0·v_L-UAV(t)+gama·distance_UAV_i(t)，得到在没有障碍物环境下的无人机跟随者的控制输入。

S106，构造分布式的编队避障结合控制器，确定在有障碍物环境下控制输入，用于异构无人集群编队避障系统在有障碍物的环境下实现编队和避障的权衡与结合；

S106具体包括：

结合S104所述的构造分布式编队形成控制器和S105所述的确定编队避障系统的权重向量，在有障碍物的环境下：

利用公式：

u_L-UGV-ob(t)＝v_L-UGV-ob(t)＝v_L-UGV(t)+σ(β·ob_dist(v,w)+γ·velocity(v,w))，确定在有障碍物环境下的无人车领航者的控制输入；

利用公式：

u_L-UAV-ob(t)＝v_L-UAV-ob(t)＝v_L-UAV(t)+σ(β·ob_dist(v,w)+γ·velocity(v,w))，确定在有障碍物环境下的无人机领航者的控制输入；

其中v_L-UGV-ob(t)∈R^2×1、v_L-UAV-ob(t)∈R^3×1分别表示无人车领航者和无人机领航者的速度信息，ob_dist(v,w)表示智能体到障碍物的距离，velocity(v,w)表示智能体的可行速度，σ、β、γ表示增益系数；

S106具体还包括：

判断所述的无人车跟随者的位置信息x_i-F-UGV(t)和所述无人机跟随者在二维平面XOY上的的位置信息x_i-F-UAV-XOY(t)，是否满足：

若满足，则说明所述的无人车跟随者和无人机跟随者ε领域内无障碍物，可按照S105中的控制输入进行编队控制；若不满足，则说明所述的无人车跟随者和无人机跟随者的ε领域内有障碍物，需要进行编队避障控制：

利用公式

确定在有障碍物环境下所述的无人车跟随者受到障碍物的斥力大小；

利用公式

确定在有障碍物环境下所述的无人机跟随者在XOY平面上受到障碍物的斥力大小；

不等式

确定所述的无人机跟随者是否受到障碍物在Z轴方向上的斥力；

若当前无人机跟随者的位置x_i-F-UAV(t)满足上述不等式，则利用公式

确定在Z轴方向上所述的无人机跟随者受到障碍物的斥力大小；若当前无人机跟随者的位置x_i-F-UAV(t)不满足上述不等式，那么所述的无人机跟随者只受到障碍物在XOY平面上的斥力，而不受到障碍物在Z轴方向上的斥力；利用公式r_i-F-UAV-ob＝r_{i-F-UAV-ob-XOY}+r_i-F-UAV-ob-Z得到所述的无人机跟随者受到的障碍物的总斥力；

利用公式

计算障碍物产生的斥力系数；

其中，r_i-F-UGV-ob∈R^2×1表示无人车跟随者在障碍物作用下所受的斥力大小，N_obs-UGV∈R^2×1表示二维空间中的障碍物位置集合，temp表示障碍物产生的斥力系数，x_m-obs-UGV∈R^{2 ×1}表示二维空间上的第m个障碍物的位置信息，r_{i-F-UAV-ob-XOY}∈R^2×1表示无人机跟随者在二维平面XOY上所受到的斥力大小，N_obs-UAV∈R^3×1表示三维空间中的障碍物位置集合，x_i-F-UAV-XOY(t)∈R^2×1表示第i个无人机跟随者在XOY平面上的位置信息，x_{m-obs-UAV-XOY}∈R^2×1表示三维空间上的第m个障碍物在XOY平面上的位置信息，r_i-F-UAV-ob-Z∈R表示无人机跟随者在Z轴方向上所受到的斥力大小，x_i-F-UAV-Z(t)∈R表示第i个无人机跟随者在Z轴方向上的位置信息，x_m-obs-UAV-Z∈R表示三维空间上的第m个障碍物在Z轴方向上的位置信息，H∈R表示无人机期望的飞行高度，r_i-F-UAV-ob∈R^3×1表示第i个无人机跟随者在障碍物作用下所受的总斥力，ob_dist表示智能体到障碍物的距离，ε为智能体需要避障时到障碍物的最小距离；

S106具体还包括：

利用公式

得到无人车跟随者和无人机跟随者的控制输入，其中，beta为增益系数；

定义编队的总误差

利用公式

确定最优的增益系数beta，用于所述的异构无人集群编队避障系统的跟随者形成最优的参考轨迹；

其中，error_formation(beta)是所述的异构无人集群编队避障系统智能体编队队形的总误差，ob_dist(beta)是智能体到障碍物的距离，δ、

是增益系数；

S106具体还包括：

判断跟随者控制u_i-F-UGV-ob(t)、u_i-F-UAV-ob(t)是否为0；

若所述的跟随者的控制输入为0，判断所述的异构无人集群编队避障系统的跟随者是否形成期望的编队队形并到达期望的位置；若所述的跟随者未形成期望的编队队形或者未到达期望的位置，则利用公式

重新规划控制输入，用于所述的异构无人集群编队避障系统的跟随者跳出局部最小值点，重新生成参考轨迹；

其中，β是增益系数。

S107，构造运动学约束模块，限制智能体的控制输入在智能体的约束范围之内；

S107具体包括：

利用公式confine_UGV＝‖v_i-UGV(k+1)‖_F-‖v_i-UGV(k)‖_F，得到无人车相邻两次规划的控制输入的差值，若confine_UGV>0，表示无人车规划的速度增大，若confine_UGV<0，表示无人车规划的速度减小；

考虑到无人车的最大加速度是有限制的，若confine_UGV>0，则利用公式v_i-UGV(k+1)＝min{v_i-UGV(k)+confine_UGV,v_i-UGV(k)+acc_UGV*dt}，若confine_UGV<0，则利用公式v_i-UGV(k+1)＝max{v_i-UGV(k)+confine_UGV,v_i-UGV(k)-acc_UGV*dt}；

考虑到无人车的最大速度是有限制的，若v_i-UGV(k+1)≥0，则利用公式v_i-UGV(k+1)＝min{v_i-UGV(k+1),v_max-UGV}，若v_i-UGV(k+1)<0，则利用公式v_i-UGV(k+1)＝max{v_i-UGV(k+1),-v_max-UGV}，从而将无人车的控制输入限制在其运动学约束范围之内；其中，confine_UGV∈R表示无人车上一时刻的速度与当前时刻速度之差，v_i-UGV(k+1)∈R^2×1、v_i-UGV(k)∈R^2×1是将v_i-UGV(t)∈R^2×1离散化的结果，分别表示第i个无人车下一时刻的速度与当前时刻的速度，acc_UGV∈R表示无人车的最大加速度，dt表示无人车规划一次控制输入需要的时间，v_max-UGV∈R表示无人车的最大速度；

同理，利用公式confine_UAV＝‖v_i-UAV(k+1)‖_F-‖v_i-UAV(k)‖_F，得到无人机相邻两次规划的控制输入的差值，若confine_UAV>0，表示无人机规划的速度增大，若confine_UAV<0，表示无人机规划的速度减小；

考虑到无人机的最大加速度是有限制的，若confine_UAV>0，则利用公式v_i-UAV(k+1)＝min{v_i-UAV(k)+confine_UAV,v_i-UAV(k)+acc_UAV*dt}，若confine_UAV<0，则利用公式v_i-UAV(k+1)＝max{v_i-UAV(k)+confine_UAV,v_i-UAV(k)-acc_UAV*dt}；

考虑到无人机的最大速度是有限制的，若v_i-UAV(k+1)≥0，则利用公式v_i-UAV(k+1)＝min{v_i-UAV(k+1),v_max-UAV}，若v_i-UAV(k+1)<0，则利用公式v_i-UAV(k+1)＝max{v_i-UAV(k+1),-v_max-UAV}，从而将无人机的控制输入限制在其运动学约束范围之内；其中，confine_UAV∈R表示无人机上一时刻的速度与当前时刻速度之差，v_i-UAV(k+1)∈R^3×1、v_i-UAV(k)∈R^3×1是将v_i-UAV(t)∈R^3×1离散化的结果，分别表示第i个无人机下一时刻的速度与当前时刻的速度，acc_UAV∈R表示无人机的最大加速度，dt表示无人机规划一次控制输入需要的时间，v_max-UAV∈R表示无人机的最大速度。

将异构无人集群编队避障方法应用到多无人机-无人车异构系统的编队避障场景，考虑由6架无人车和6架无人机组成的异构无人集群系统。多无人车和多无人机能够形成期望的编队队形，且6架无人机期望在指定的高度进行飞行，同时无人机领航者跟随无人车领航者。无人车只能在OXY二维平面内运动，无人机可以在OXYZ三维空间内运动，要求无人车在OXY平面内进行编队，期望的编队位置向量可以表示为

其中，h_UGV-1＝[10.5,13.5]^T，h_UGV-2＝[13.5,13.5]^T，h_UGV-3＝[9,0]^T，h_UGV-4＝[10.5,10.5]^T，h_UGV-5＝[13.5,10.5]^T，h_UGV-6＝[12,12]^T，h_UGV-6表示无人车领航者的期望位置；

其中，h_UAV-1＝[12.5,10.5,1]^T，h_UAV-2＝[13.3,12,1]^T，h_UAV-3＝[12.5,13.5,1]^T，h_UAV-4＝[11,13,1]^T，h_UAV-5＝[11,11,1]^T，h_UAV-6＝[12,12,1]^T，h_UAV-6表示无人机领航者的期望位置；障碍物的位置

其中，h_obs-1＝[5,5,0]^T，h_obs-2＝[8,6,0]^T，h_obs-3＝[6,5.5,1]^T，h_obs-4＝[9,6.5,1.4]^T。

通信拓扑关系如图2和图3所示，其中，图2表示无人车的通信拓扑关系，图3表示无人机的通知拓扑关系，图中的箭头表示智能体与智能体之间可以进行通信，若两智能体之间没有箭头连接，表示该对智能体不互为邻居，无法进行通信。箭头的方向都是双向的，表示智能体与智能体之间的通信是双向的。

图4和图5表示智能体初始时刻和终止时刻的相对位置图，图中共有12个圆圈，代表12个智能体节点，其中灰色的节点表示领航者，黑色节点表示跟随者，在OXY平面上的6个节点表示无人车，在OXYZ三维空间中的6个节点表示无人机，节点与节点之间的双向箭头表示该对节点可以进行双向通信，节点上的线段表示该智能体的运动方向，障碍物使用“×”来表示；其中图4表示异构无人集群编队避障系统中智能体的初始位置，图5表示智能体的终止位置。

图6表示智能体的运动轨迹图，图中共有12条智能体的运动轨迹，在OXY平面上的6条轨迹表示无人车的运动轨迹，在OXYZ三维空间中的6条轨迹表示无人机的运动轨迹，实心圆代表无人车领航者的初始位置，“*”代表无人机领航者的初始位置，实心五角星代表无人车跟随者和无人机跟随者的初始位置，空心圆代表无人车领航者和无人机领航者的终止位置，空心三角形代表无人车跟随者和无人机跟随者的终止位置，障碍物使用“×”来表示。

图7-图11为智能体的速度变化曲线图，横轴表示速度迭代的次数，纵轴表示相应的速度大小；其中图7表示无人车在X轴方向上的速度曲线，图8表示无人车在Y轴方向上的速度曲线，图9表示无人机在X轴方向上的速度，图10表示无人机在Y轴方向上的速度，图11表示无人机在Z轴方向上的速度；图7-图11表明智能体经过避障之后可以达到速度一致性并且形成期望的编队队形。

图12和图13为智能体编队避障的权重累加图，其中图12表示无人车编队避障的权重累加图，用于衡量无人车在编队避障过程中每架无人车与期望编队队形的偏移程度；图13表示无人机编队避障的权重累加图，用于衡量无人机在编队避障过程中每架无人机与期望编队队形的偏移程度。

图14为本发明提供的一种异构无人编队避障系统的结构示意图，包括：

选择领航者模块701，用于异构无人集群编队避障系统确定无人车领航者和无人机领航者，所述的无人车领航者和无人机领航者分别是异构无人集群编队避障系统中具有异构性的不同类型的智能体；

通信拓扑模块702，用于构建异构无人集群编队避障系统的通信拓扑关系，所述的异构无人集群编队避障系统中的智能体只能与同构的邻居智能体进行通信，且智能体与智能体之间的作用拓扑是双向的；

编队避障模块703，用于生成所述的异构无人集群编队避障系统编队的参考轨迹；所述的编队避障模块具体包括编队形成模块和编队避障结合模块，其中，编队形成模块用于所述的异构无人集群编队避障系统的编队队形的形成，编队避障结合模块用于所述的异构无人集群编队避障系统在避障的过程中保持编队队形；所述的编队避障模块包含升降模块，用于所述的异构无人集群编队避障系统中的无人机在不改变编队队形的情况下躲避障碍物，实现在垂直于地面的方向上避障；

空地协同模块704，用于保证无人车和无人机之间协同进行编队；

运动学约束模块705，用于保证智能体的控制输入限制在相应智能体的运动学约束范围之内。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

尽管本文较多地使用了同构、异构、智能体等术语，但并不排除使用其它术语的可能性。使用这些术语仅仅是为了更方便地描述和解释本发明的本质；把它们解释成任何一种附加的限制都是与本发明精神相违背的。

Claims (9)

1.一种异构无人集群编队避障方法，其特征在于，包括以下步骤：

S101、获取异构无人集群编队避障系统的通信拓扑关系，确定编队系统的稳定性；所述的异构无人集群编队避障的通信拓扑关系包括无人车之间的通信拓扑图和无人机之间的通信拓扑图，智能体只能与同构的邻居智能体进行通信，且智能体与智能体之间的作用拓扑是双向的；

S102、根据异构无人集群编队避障系统中智能体的运动状态和控制输入，确定智能体的运动学模型；

S103、根据智能体的运动状态和障碍物的位置信息，选择无人车领航者和无人机领航者；

S104、构造分布式的编队形成控制器，确定在无障碍物环境下的控制输入，所述的编队形成控制器生成所述的异构无人集群编队避障系统在无障碍物环境下整体运动的参考轨迹；

S105、根据智能体的位置信息，确定编队避障系统的权重向量，并利用权重向量优化无障碍物环境下整体运动的参考轨迹；

S106、构造分布式的编队避障结合控制器，确定在有障碍物环境下控制输入，对异构无人集群编队避障系统在有障碍物的环境下实现编队和避障的权衡与结合；

S107、构造运动学约束模块，限制智能体的控制输入在智能体的约束范围之内。

2.根据权利要求1所述的一种异构无人集群编队避障方法，其特征在于，步骤S101具体包括：

同构无人集群编队的通信拓扑用图

表示，

表示节点集合，

表示边集合，

表示第i个智能体与第j个智能体的通信关系；令ε_ij＝(v_i，v_j)表示图

中从节点v_i到节点v_j的一条边；当且仅当第i个智能体与第j个智能体互为邻居节点，即第i个智能体与第j个智能体之间可以进行通信时，令ω_ij＝1，否则ω_ij＝0；用

表示节点v_i的邻居节点集合；将图

的邻接矩阵定义为

将图

的入度矩阵定义为

其中，

定义图的拉普拉斯矩阵为

定义矩阵

的约当标准形式为

其中，矩阵

式中λ_i为矩阵

的特征值，V_i为矩阵

的右特征向量，满足

其中

为单位矩阵；矩阵

的左特征向量ω_i满足

矩阵

的任意一行元素之和都为0，所以当且仅当

时，特征值为0，其中c为唯一特征向量，此时

的秩为N-1，从而使得拓扑图

可以有一个生成树，系统是可以稳定的。

3.根据权利要求2所述的一种异构无人集群编队避障方法，其特征在于，步骤S102具体包括：

首先利用公式

确定无人车的运动模型；

利用公式

确定无人机的运动模型；

其中，x_i-UGV(t)∈R^2×1是第i个无人车节点的位置信息，

是x_i-UGV(t)的微分，u_i-UGV(t)∈R^2×1为第i个无人车节点的控制输入量，v_i-UGV(t)∈R^2×1为第i个无人车节点的速度信息，x_i-UAV(t)∈R^3×1是第i个无人机节点的位置信息，

是x_i-UAV(t)的微分，u_i-UAV(t)∈R^3×1为第i个无人机节点的控制输入量，v_i-UAV(t)∈R^3×1为第i个无人机节点的速度信息。

4.根据权利要求3所述的一种异构无人集群编队避障方法，其特征在于，步骤S103具体包括：

初始时刻根据通信拓扑图

随机选择智能体作为所述的无人集群编队避障系统的无人车领航者和无人机领航者；

根据所述的异构无人集群编队避障系统的运动状态以及环境中障碍物的位置，利用公式

得到可以作为领航者的智能体集合，判断所述的随机选择的无人车领航者和无人机领航者是否在可以作为领航者的智能体集合L_i中；

若所述的随机选择的无人车领航者或无人机领航者不在所述的可以作为领航者的智能体的集合L_i中，则认为当前随机选择的无人车领航者或无人机领航者无效，那么重新在所述的可以作为领航者的智能体集合L_i中随机选择异构无人集群编队避障系统的无人车领航者或无人机领航者；若所述的随机选择的无人车领航者和无人机领航者在所述的可以作为领航者的智能体的集合L_i中，则认为当前随机选择的无人车领航者和无人机领航者有效；

其中，ε为智能体需要避障时到障碍物的最小距离，x_obs-UGV∈R^2×1表示二维空间上障碍物的位置信息，x_obs-UAV∈R^3×1表示三维空间上障碍物的位置信息，N为同构智能体的数量。

5.根据权利要求4所述的一种异构无人集群编队避障方法，其特征在于，步骤S104具体包括：

利用公式v_L-UGV(t)＝u_L-UGV(t)＝KN*(x_goal-UGV-x_L-UGV(t))，确定在没有障碍物环境下的无人车领航者的控制输入；

利用公式v_L-UAV(t)＝u_L-UAV(t)＝KN*(x_goal-UAV-x_L-UAV(t))，确定在没有障碍物环境下的无人机领航者的控制输入；

令x_goal-UAV＝[x_L-UGV(t)；H]，无人机领航者的目标位置信息即为无人车领航者的位置信息对无人机期望的飞行高度H的增广，实现无人机领航者对无人车领航者的跟踪，保证空地智能体协同进行编队；

其中，H表示无人机期望的飞行高度；

利用公式

确定在没有障碍物环境下的无人车跟随者的控制输入；

利用公式

确定在没有障碍物环境下的无人机跟随者的控制输入；

其中，v_L-UGV(t)∈R^2×1是无人车领航者的速度信息，u_L-UGV(t)∈R^2×1是无人车领航者的控制输入量，KN表示领航者跟踪目标增益系数，x_goal-UGV∈R^2×1表示无人车领航者的目标点，x_L-UGV(t)∈R^2×1是无人车领航者的位置信息，v_L-UAV(t)∈R^3×1是无人机领航者的速度信息，u_L-UAV(t)∈R^3×1是无人机领航者的控制输入量，x_goal-UAV∈R^3×1表示无人机领航者的目标点，x_L-UAV(t)∈R^3×1是无人机领航者的位置信息，v_i-F-UGV(t)∈R^2×1是第i个无人车跟随者的速度信息，u_i-F-UGV(t)∈R^2×1表示第i个无人车跟随者的控制输入，K0表示一致性增益系数，ω_ij表示权重，N_i表示与第i个智能体互为邻居的智能体的集合，x_i-F-UGV(t)∈R^2×1表示第i个无人车跟随者的位置信息，x_j-UGV(t)∈R^2×1表示第j个无人车的位置信息，

表示第i个无人车跟随者的期望位置，

表示第j个无人车的期望位置，v_i-F-UAV(t)∈R^3×1是第i个无人机跟随者的速度信息，u_i-F-UAV(t)∈R^3×1表示第i个无人机跟随者的控制输入，x_i-F-UAV(t)∈R^3×1表示第i个无人机跟随者的位置信息，x_j-UAV(t)∈R^3×1表示第j个无人机的位置信息，

表示第i个无人机跟随者的期望位置，

表示第j个无人机的期望位置；

S104具体还包括：

利用公式

其中j∈N_i，得到第i个无人车与其邻居智能体形成期望编队的误差；

利用公式

其中j∈N_i，得到第i个无人机与其邻居智能体形成期望编队的误差；

利用公式distance_UGV_i(t)＝(sum_delta_UGV_F_i(t)^T·sum_delta_UGV_F_i(t))^1/2，得到第i个无人车与其邻居智能体形成期望编队的误差距离；

利用公式distance_UAV_i(t)＝(sum_delta_UAV_F_i(t)^T·sum_delta_UAV_F_i(t))^1/2，得到第i个无人机与其邻居智能体形成期望编队的误差距离；

利用公式v_i-F-UGV(t)＝u_i-F-UGV(t)＝K0·v_L-UGV(t)+gama·distance_UGV_i(t)，得到在没有障碍物环境下的无人车跟随者的控制输入；

利用公式v_i-F-UAV(t)＝u_i-F-UAV(t)＝K0·v_L-UAV(t)+gama·distance_UAV_i(t)，得到在没有障碍物环境下的无人机跟随者的控制输入；

其中，sum_delta_UGV_F_i(t)∈R^2×1表示第i个无人车与其邻居智能体形成期望编队的误差，A(i，j)表示第i个无人车和第j个无人车的通信拓扑关系，当第i个无人车和第j个无人车存在通信时A(i，j)＝1，否则A(i，j)＝0，sum_delta_UAV_F_i(t)∈R^3×1表示第i个无人机与其邻居智能体形成期望编队的误差，B(i，j)表示第i个无人机和第j个无人机的通信拓扑关系，当第i个无人机和第j个无人机存在通信时B(i，j)＝1，否则B(i，j)＝0，distance_UGV_i(t)∈R表示第i个无人车与其邻居智能体形成期望编队的误差距离，distance_UAV_i(t)∈R表示第i个无人机与其邻居智能体形成期望编队的误差距离，gama为跟随者跟踪目标增益系数。

6.根据权利要求5所述的一种异构无人集群编队避障方法，其特征在于，步骤S105具体包括：

利用公式ω_UGV_ij＝2-exp(-sum_delta_UGV_F_i(t)^T·sum_delta_UGV_F_i(t))，得到第i个无人车用于编队形成控制的权重；

利用公式ω_UAV_ij＝2-exp(-sum-delta_UAV_F_i(t)^T·sum_delta_UAV_F_i(t))，得到第i个无人机用于编队形成控制的权重；

其中，ω_UGV_ij∈R为第i个无人车用于编队形成控制的权重，ω_UAV_ij∈R为第i个无人机用于编队形成控制的权重；

将S105中的权重向量用在S104的编队形成控制器中，生成在无障碍物环境下的编队整体运动的优化参考轨迹；

利用公式

其中j∈N_i，得到第i个无人车与其邻居智能体形成期望编队的误差最小值；

利用公式

其中j∈N_i，得到第i个无人机与其邻居智能体形成期望编队的误差最小值；

利用公式distance_UGV_i(t)＝(sum_delta_UGV_F_i(t)^T·sum_delta_UGV_F_i(t))^1/2，得到第i个无人车与其邻居智能体形成期望编队的最小误差距离之和；

利用公式distance_UAV_i(t)＝(sum_delta_UAV_F_i(t)^T·sum_delta_UAV_F_i(t))^1/2，得到第i个无人机与其邻居智能体形成期望编队的最小误差距离之和；

利用公式v_i-F-UGV(t)＝u_i-F-UGV(t)＝K0·v_L-UGV(t)+gama·distance_UGV_i(t)，得到在没有障碍物环境下的无人车跟随者的控制输入；

利用公式v_i-F-UAV(t)＝u_i-F-UAV(t)＝K0·v_L-UAV(t)+gama·distance_UAV_i(t)，得到在没有障碍物环境下的无人机跟随者的控制输入。

7.根据权利要求6所述的一种异构无人集群编队避障方法，其特征在于，步骤S106具体包括：

结合S104所述的构造分布式编队形成控制器和S105所述的确定编队避障系统的权重向量，在有障碍物的环境下：

利用公式：u_L-UGV-ob(t)＝v_L-UGV-ob(t)＝v_L-UGV(t)+σ(β·ob_dist(v，w)+γ·velocity(v，w))，确定在有障碍物环境下的无人车领航者的控制输入；

利用公式：u_L-UAV-ob(t)＝v_L-UAV-ob(t)＝v_L-UAV(t)+σ(β·ob_dist(v，w)+γ·velocity(v，w))，确定在有障碍物环境下的无人机领航者的控制输入；

其中v_L-UGV-ob(t)∈R^2×1、v_i-UAV-ob(t)∈R^3×1分别表示无人车领航者和无人机领航者的速度信息，ob_dist(v，w)表示智能体到障碍物的距离，velocity(v，w)表示智能体的可行速度，σ、β和γ表示动态窗口避障增益系数；

S106具体还包括：

判断所述的无人车跟随者的位置信息x_i-F-UGV(t)和所述无人机跟随者在二维平面XOY上的的位置信息x_i-F-UAV-XOY(t)，是否满足：

若满足，则说明所述的无人车跟随者和无人机跟随者ε领域内无障碍物，可按照S105中的控制输入进行编队控制；若不满足，则说明所述的无人车跟随者和无人机跟随者的ε领域内有障碍物，需要进行编队避障控制：

利用公式

确定在有障碍物环境下所述的无人车跟随者受到障碍物的斥力大小；

利用公式

确定在有障碍物环境下所述的无人机跟随者在XOY平面上受到障碍物的斥力大小；

利用不等式

确定所述的无人机跟随者是否受到障碍物在Z轴方向上的斥力；

若当前无人机跟随者的位置x_i-F-UAV(t)满足上述不等式，则利用公式

确定在Z轴方向上所述的无人机跟随者受到障碍物的斥力大小；若当前无人机跟随者的位置x_i-F-UAV(t)不满足上述不等式，那么所述的无人机跟随者只受到障碍物在XOY平面上的斥力，而不受到障碍物在Z轴方向上的斥力；

利用公式r_i-F-UAV-ob＝r_{i-F-UAV-ob-XOY}+r_i-F-UAV-ob-z得到所述的无人机跟随者受到的障碍物的总斥力；

利用公式

计算障碍物产生的斥力系数；

其中，r_i-F-UGV-ob∈R^2×1表示无人车跟随者在障碍物作用下所受的斥力大小，N_obs-UGV∈R^{2 ×1}表示二维空间中的障碍物位置集合，temp表示障碍物产生的斥力系数，x_m-obs-UGV∈R^2×1表示二维空间上的第m个障碍物的位置信息，r_{i-F-UAV-ob-XOY}∈R^2×1表示无人机跟随者在二维平面XOY上所受到的斥力大小，N_obs-UAV∈R^3×1表示三维空间中的障碍物位置集合，x_i-F-UAV-XOY(t)∈R^2×1表示第i个无人机跟随者在XOY平面上的位置信息，x_{m-obs-UAV-XOY}∈R^2×1表示三维空间上的第m个障碍物在XOY平面上的位置信息，r_i-F-UAV-ob-z∈R表示无人机跟随者在Z轴方向上所受到的斥力大小，x_i-F-UAV-Z(t)∈R表示第i个无人机跟随者在Z轴方向上的位置信息，x_m-obs-UAV-Z∈R表示三维空间上的第m个障碍物在Z轴方向上的位置信息，H∈R表示无人机期望的飞行高度，r_i-F-UAV-ob∈R^3×1表示第i个无人机跟随者在障碍物作用下所受的总斥力，ob_dist表示智能体到障碍物的距离，ε为智能体需要避障时到障碍物的最小距离；

S106具体还包括：

利用公式

得到无人车跟随者和无人机跟随者的控制输入，其中，beta为人工势场增益系数；

定义编队的总误差

利用公式

确定最优的人工势场增益系数beta，用于所述的异构无人集群编队避障系统的跟随者形成最优的参考轨迹；

其中，error_formation(beta)是所述的异构无人集群编队避障系统智能体编队队形的总误差，ob_dist(beta)是智能体到障碍物的距离，δ、

是编队误差权重增益系数；

S106具体还包括：

判断跟随者控制u_i-F-UGV-ob(t)、u_i-F-UAV-ob(t)是否为0；

若所述的跟随者的控制输入为0，判断所述的异构无人集群编队避障系统的跟随者是否形成期望的编队队形并到达期望的位置；若所述的跟随者未形成期望的编队队形或者未到达期望的位置，则利用公式

重新规划控制输入，用于所述的异构无人集群编队避障系统的跟随者跳出局部最小值点，重新生成参考轨迹；

其中，β为前述的动态窗口避障增益系数。

8.根据权利要求7所述的一种异构无人集群编队避障方法，其特征在于，步骤S107具体包括：

利用公式confine_UGV＝||v_i-UGV(k+1)||_F-||v_i-UGV(k)||_F，得到无人车相邻两次规划的控制输入的差值，若confine_UGV＞0，表示无人车规划的速度增大，若confine_UGV＜0，表示无人车规划的速度减小；

考虑到无人车的最大加速度是有限制的，若confine_UGV＞0，则利用公式v_i-UGV(k+1)＝min{v_i-UGV(k)+confine_UGV，v_i-UGV(k)+acc_UGV*dt}，若confine_UGV＜0，则利用公式v_i-UGV(k+1)＝max{v_i-UGV(k)+confine_UGV，v_i-UGV(k)-acc_UGV*dt}；

考虑到无人车的最大速度是有限制的，若v_i-UGV(k+1)≥0，则利用公式v_i-UGV(k+1)＝min{v_i-UGV(k+1)，v_max-UGV}，若v_i-UGV(k+1)＜0，则利用公式v_i-UGV(k+1)＝max{v_i-UGV(k+1)，-v_max-UGV}，从而将无人车的控制输入限制在其运动学约束范围之内；

其中，confine_UGV∈R表示无人车上一时刻的速度与当前时刻速度之差，v_i-UGV(k+1)∈R^2×1、v_i-UGV(k)∈R^2×1是将v_i-UGV(t)∈R^2×1离散化的结果，分别表示第i个无人车下一时刻的速度与当前时刻的速度，acc_UGV∈R表示无人车的最大加速度，dt表示无人车规划一次控制输入需要的时间，v_max-UGV∈R表示无人车的最大速度；

同理，利用公式confine_UAV＝||v_i-UAV(k+1)||_F-||v_i-UAV(k)||_F，得到无人机相邻两次规划的控制输入的差值，若confine_UAV＞0，表示无人机规划的速度增大，若confine_UAV＜0，表示无人机规划的速度减小；

考虑到无人机的最大加速度是有限制的，若confine_UAV＞0，则利用公式v_i-UAV(k+1)＝min{v_i-UAV(k)+confine_UAV，v_i-UAV(k)+acc_UAV*dt}，若confine_UAV＜0，则利用公式v_i-UAV(k+1)＝max{v_i-UAV(k)+confine_UAV，v_i-UAV(k)-acc_UAV*dt}；

考虑到无人机的最大速度是有限制的，若v_i-UAV(k+1)≥0，则利用公式v_i-UAV(k+1)＝min{v_i-UAV(k+1)，v_max-UAV}，若v_i-UAV(k+1)＜0，则利用公式v_i-UAV(k+1)＝max{v_i-UAV(k+1)，-v_max-UAV}，从而将无人机的控制输入限制在其运动学约束范围之内；

其中，confine_UAV∈R表示无人机上一时刻的速度与当前时刻速度之差，v_i-UAV(k+1)∈R^3×1、v_i-UAV(k)∈R^3×1是将v_i-UAV(t)∈R^3×1离散化的结果，分别表示第i个无人机下一时刻的速度与当前时刻的速度，acc_UAV∈R表示无人机的最大加速度，dt表示无人机规划一次控制输入需要的时间，v_max-UAV∈R表示无人机的最大速度。

9.一种异构无人集群编队避障系统，运行有权利要求1所述的异构无人编队避障方法，其特征在于，包括：

选择领航者模块701，用于异构无人集群编队避障系统确定无人车领航者和无人机领航者，所述的无人车领航者和无人机领航者分别是异构无人集群编队避障系统中具有异构性的不同类型的智能体；

通信拓扑模块702，用于构建异构无人集群编队避障系统的通信拓扑关系，所述的异构无人集群编队避障系统中的智能体只能与同构的邻居智能体进行通信，且智能体与智能体之间的作用拓扑是双向的；

编队避障模块703，用于生成所述的异构无人集群编队避障系统编队的参考轨迹；所述的编队避障模块具体包括编队形成模块和编队避障结合模块，其中，编队形成模块用于所述的异构无人集群编队避障系统的编队队形的形成，编队避障结合模块用于所述的异构无人集群编队避障系统在避障的过程中保持编队队形；所述的编队避障模块包含升降模块，用于所述的异构无人集群编队避障系统中的无人机在不改变编队队形的情况下躲避障碍物，实现在垂直于地面的方向上避障；

空地协同模块704，用于保证无人车和无人机之间协同进行编队；

运动学约束模块705，用于保证智能体的控制输入限制在相应智能体的运动学约束范围之内。

Images