同步实现无人机集群定位与仿射编队跟踪控制的方法及装置

本发明涉及多智能体协同控制领域，尤其涉及一种同步实现无人机集群定位与仿射编队跟踪控制的方法及装置。

背景技术：

1、为实现无人机集群的多智能体协同控制，需要使用分布式协同控制算法，目前存在众多不同类型的分布式协同控制算法。根据测量信息的不同，分布式控制算法可分为基于位置、基于位移、基于距离以及基于方位等的控制算法，其中基于方位测量的编队控制方法主要依赖于方位测量信息，不仅能够实现较好的控制效果，且信息获取的成本低廉、实现简单。

2、但是现有技术中基于方位测量信息的协同控制算法，通常需要假设期望的编队是无穷小方位刚性的，即只允许编队进行无穷小方位机动(包括平移运动和缩放运动)以保证智能体之间的方位保持恒定，从而保证可以基于方位信息唯一地确定编队构型，而这大大限制了多智能体系统的机动形式，难以满足复杂多变的任务要求。此外，多数协同控制算法只适用于线性系统，例如一阶积分系统和二阶积分系统，难以直接拓展到固定翼无人机集群中。完整的固定翼无人机运动与动力学特性可以由十二维状态耦合的非线性方程描述，这太过于复杂而难以用于协同控制策略设计。常见的固定翼无人机简化模型为二维独轮车模型，它假设无人机飞行高度恒定，从而将无人机的三维机动转化为二维运动；然而，定高假设与无人机的真实运动形式有较大差距，会提高协同算法实际应用的难度。

3、综上，针对传统固定翼无人机集群的协同编队控制，一方面，传统基于方位的协同控制算法仅能够实现无人机集群固定构型的控制，无人机集群的机动形式受限，在缺乏位置信息时，无法同时实现固定翼无人机集群的定位以及仿射编队控制；另一方面，传统基于方位的协同控制算法通常是针对线性模型，难以直接应用至高度非线性的三维固定翼无人机集群中，不能很好地通过分布式协同控制实现目标任务。

技术实现思路

1、本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种实现简单、成本低且控制灵活、机动形式多样、可扩展性强的同步实现无人机集群定位与仿射编队跟踪控制的方法及装置，能够在不完全依赖全局位置的前提下，同步实现无人机集群定位与仿射编队跟踪控制，可以有利于应用于无人集群在感知受限环境中。

2、为解决上述技术问题，本发明提出的技术方案为：

3、一种同步实现无人机集群定位与仿射编队跟踪控制的方法，包括以下步骤：

4、步骤s01.对固定翼无人机集群进行建模，无人机之间的拓扑连接关系使用无向连通图表示，构建满足预设协同控制要求的标称编队r表示标称构型，所述标称编队中选取多架在空间中不共面的无人机作为领航者、其余无人机作为跟随者，对所述标称编队进行仿射变换得到期望的编队构型；

5、步骤s02.求解所述标称编队的平衡应力矩阵，所述平衡应力矩阵为使得图中各边分配的应力达到平衡的应力矩阵；

6、步骤s03.在固定翼无人机集群飞行过程中，控制领航者实时获取自身位置信息，并通过机间通信将位置信息共享给邻居跟随者，跟随者实时测量与邻居无人机之间的方位信息，并接收来自领航者邻居以及来自跟随者邻居的位置信息对自身位置的估计值，根据实时测量得到的方位信息以及接收到的信息估计跟随者自身位置信息，并共享给邻居无人机；

7、步骤s04.根据实时估计出的自身位置信息以及所述平衡应力矩阵实时计算出跟随者的控制输入，以控制跟随者跟踪领航者，同时控制无人机集群收敛到期望的编队构型，实现无人机集群的仿射编队控制。

8、进一步的，所述步骤s01包括：

9、步骤s101.将固定翼无人机集群建模为其中v＝{1,2…n}代表图中节点的集合，代表图中边的集合，并建立固定翼无人机的简化运动学模型；

10、步骤s102.定义满足普遍刚性以及仿射可定位性的标称构型得到无人机集群的标称编队为其中，如果标称构型r在所有维空间上均为图g关于刚性运动的唯一实现则判定标称构型r满足所述普遍刚性，如果对于处于r的仿射映射中的任意构型pf可以被pl唯一确定，其中中包含了r的所有仿射变换，则判定标称构型r满足所述仿射可定位性；

11、步骤s103.对所述标称编队进行仿射变换，并使得仿射变换后保持构型中节点之间共线或共面的特性不变以及相互平行的线段在仿射变换后保持平行，得到期望的编队构型。

12、进一步的，所述固定翼无人机的简化运动学模型为：

13、

14、其中，pi表示无人机i的位置，pi＝[xi,yi,zi]t，ui＝[vi,0,0]t，vi表示无人机i的线速度，hi表示ri的第一列航向向量变量，τi表示无人机i的虚拟线加速度，τi和ωi为无人机的两个虚拟控制输入；

15、令χi＝-αi，αi表示无人机i飞行航迹角，ri表示无人机i的旋转矩阵：

16、

17、其中，βi表示无人机i的航向角；

18、在惯性坐标系中定义角速度为令(·)∧:则

19、

20、

21、其中，因此，

22、进一步的，步骤s103中，对所述标称编队进行仿射变换后得到无人机i的时变期望位置为：

23、

24、其中，a(t)表示可逆矩阵平移向量b(t)对于时间t连续且ri表示无人机i的仿射变换。

25、进一步的，步骤s02中，给所述无向连通图中的边(i,j)∈ε，随机分配应力且当存在任意节点i满足时，则为平衡应力，节点i、j分别表示无人机i、无人机j，应力矩阵定义为：

26、

27、其中，ω是半正定矩阵，rank(ω)＝n-4，且是正定矩阵。

28、进一步的，根据所述应力矩阵构建应力矩阵求解问题，给所述无向连通图的每一条边随机指定一个方向构建得到有向图，所述有向图中边的方向用关联矩阵表示，关联矩阵h的第i列为bi，即h＝[b1,…bn]，定义如下矩阵：

29、

30、

31、对进行奇异值分解得到令u＝[u1,u2]；

32、定义：

33、

34、构建如下的线性矩阵不等式：

35、

36、其中c1,…cq满足

37、求解所述线性矩阵不等式得到名义编队的平衡应力以及平衡应力矩阵：其中diag表示对角矩阵。

38、进一步的，所述步骤s03中，定义矩阵π＝i3-φφt，其中向量eij表示边{i,j}之间的方位eij＝pj-pi，方位向量按照下式估计得到跟随者无人机i的自身位置信息以进行同步定位：

39、

40、其中，kδ与kp是正的控制增益；为跟随者无人机对自身位置的估计值；向量gij为无人机i与其余邻居无人机j之间的方位向量。

41、进一步的，所述步骤s04中，基于仿射编队控制按照下式计算出跟随者的虚拟控制输入τi和ωi以进行仿射编队协同跟踪控制：

42、

43、

44、

45、

46、

47、其中，kp和kv为正的控制增益；fi表示跟随者无人机的控制输入；hi表示航向向量；ψi(t)表示扰动项，向量表示前馈项，用于补偿无人机的加速度以消除编队跟踪误差，表示估计方位误差的和；κi(t)表示连续的时变向量；表示跟随者无人机邻居集合，表示领导者无人机邻居集合，表示平衡应力，表示无人机i的位置信息，表示无人机j的位置估计，pj表示无人机j的位置信息，τi表示无人机的虚拟线加速度，函数sgn(·)为符号函数ψi(t)表示扰动项；向量gij为无人机i与邻居无人机j之间的方位向量；表示无人机i与邻居无人机j之间的方位向量的估计值。

48、进一步的，所述步骤s04中，还包括将计算出跟随者的虚拟控制输入τi和ωi转换为跟随者无人机的真实控制输入，转换计算公式为：

49、ti＝miτi+fi-migsinχi

50、

51、

52、其中，mi表示无人机i的质量，fi表示空气阻力，g表示中立加速度，χi表示无人机i的姿态角，vi表示无人机i的速度，ti表示无人机i推力，li表示无人机i的升力，表示无人机i的倾斜角。

53、一种同步实现无人机集群定位与仿射编队跟踪控制的装置，包括控制模块以及布置在无人机集群中各无人机上的通信模块、导航定位模块、传感器模块，所述控制模块包括处理器以及存储器，所述存储器用于存储计算机程序，所述处理器用于执行所述计算机程序以执行如上述方法。

54、与现有技术相比，本发明的优点在于：

55、1、本发明在对固定翼无人机集群进行控制过程中，通过求解标称编队的平衡应力矩阵，同时控制跟随者利用与邻居无人机之间的方位信息估计自身位置信息，根据估计出的自身位置信息以及平衡应力矩阵计算出跟随者的控制输入，能够实现同步定位与协同控制的耦合，控制无人机集群灵活的收敛到期望的编队构型，使得在缺乏位置信息的前提下，利用方位测量信息即可同步实现固定翼无人机集群的定位以及仿射编队控制，大大增强了非线性固定翼无人机集群的机动灵活性以及可扩展性。

56、2、本发明不完全依赖无人机的位置测量信息，利用方位测量信息即可同步实现无人机定位以及仿射编队跟踪控制，不仅可以降低实现成本以及实现复杂读，还可以有效提高环境适应性，便于应用于无人集群在感知受限环境下得应用场景中。

57、3、本发明通过采用仿射编队协同控制方式，控制少量节点即可实现期望的协同控制目标，同时跟随者能够自主跟踪领航者自主飞行并收敛到期望的编队构型，大大减小无人机操控员的工作负担，使得可以方便得执行对于无人机多机编队协同遂行任务。

58、4、本发明进一步采用三维六自由度运动学模型对固定翼无人机集群进行建模，能够准确刻画影响无人机运动的关键、本质因素，使得模型更加贴近现实情况，从而便于迁移到真实世界；进一步通过对无人机运动学模型进行了等效转换，构建虚拟控制输入与真实控制输入之间的映射关系，还能够在保留控制策略真实性的基础上，降低非线性多智能体系统协同控制算法的设计难度。