0 引言

多机器人编队控制是多机器人协调控制的一个重要研究方向，广泛应用在巡逻、搜救、侦察、空间探测等领域.传统的编队方法主要有Leader-Follower法^[1]、基于行为法^[2]、虚拟结构法^[3]，另外部分研究者将图论和势场函数的知识融入到编队控制方法中.

在编队拓扑控制研究中，刚性图论被广泛应用，利用机器人之间的距离约束，设计编队控制规律来保持队形刚性或近似刚性，取得了较大进展.在刚性图的基础上，Hendrickx等在文[4]中提出了有向刚性的概念，并给出了生成有向刚性图的策略；在文[5]中将2维刚性图延伸到3维甚至更高维数，并给出刚性图与有向图持久性的充分必要条件；在文[6-7]中提出了利用基础有向操作实现最小持久编队的方法；但上述研究仅停留在理论层面且不能保证生成预期持久编队队形.邵士凯等^[8]提出基础圈的概念，并研究了基础圈为三角形或四边形的最优持久编队分布式生成算法，但该方法只适用于2维空间且通信复杂度较高.王佳等^[9]提出一种针对移动障碍物的编队控制方法，在二次积分模型的基础上，采用势能函数进行控制设计.此方法减小了整个势能场的复杂度，降低了局部极小值产生的可能性，但该方法只适用于移动障碍物存在的情况. Ramazani^[10-11]等针对不同平面多智能体间的协同控制问题，利用刚性图论分别对单积分和双积分模型进行了研究，但该方法对智能体的初始位置有较大约束.

目前刚性图论研究大多停留在理论层面且编队对象多为理想积分模型，实际应用于多机器人编队仍待进一步研究.并且，机器人编队时位置的精确性会对其产生一定的影响，目前机器人编队时大部分都采用自身的里程计，但是该里程计提供的位置信息的精度有待提高.

综合上述分析，本文提出一种基于视觉SLAM定位和结构持久图的多机器人编队生成与控制方法.本文的主要工作和创新点主要为：

1) 首先，基于结构持久图的概念，同时考虑全局刚性的问题，提出改进的有向增加顶点操作设计多机器人最小持久编队；其次，针对多机器人分别设计势场函数进行控制，同时考虑双轮机器人运动模型的非完整性，引入虚拟机器人，提出一种双轮机器人最小持久编队方法.本文提出的算法能够保证机器人的编队更加灵活和稳定.

2) 针对编队过程中需要的机器人位置信息，提出一种基于RGB-D相机的视觉SLAM算法，为编队控制提供精确的位置信息；该算法能够保证位置信息的精确，使得在编队过程中队形能够更好的形成和保持.

3) 结合无人机和多机器人系统，提出不同平面多智能体的最小持久编队方法并验证所提理论的有效性.本文所提的算法解决了不同平面下多智能体的编队问题，对于推动不同平面多智能体的协同控制的发展具有一定的意义.

1 图论基础

多机器人编队包括移动机器人和机器人之间的通信网络.令G=(V，E)表示多机器人编队对应的基础图，其中V={V₁，V₂，...，V_n}表示图中各点的集合，对应于编队中的多机器人；E⊂V×V是图中边的集合，对应于机器人之间的通信连接；能够通信的机器人互为邻居.如果图G的任意顶点对(V_i，V_j)满足∀(V_i，V_j)∈E，则(V_j，V_i)∈E，则图G为无向图，否则为有向图，记为.在无向图中，邻接矩阵A=(a_ij)(a_ii=0；当(V_i，V_j)∈E，a_ij=a_ji=1，否则a_ij=0)，机器人V_i的邻居集表示为N_i={V_j:(V_i，V_j)∈E}；在有向图中，任一有向边，表示机器人V_i相对于机器人V_j的位置约束，此时顶点V_i的出度为1，记为d⁺(i)=1，顶点V_j的入度为1，记为d^-(i)=1.机器人V_i的有向邻居集表示为中，机器人V_i的自由度可表示为max(0，d-d⁺(i)).

1.1 刚性图及分类

在机器人编队中，q₁，q₂，…，q_n∈R^d，d∈{2，3}表示机器人的坐标，q_i(t)(t≥0)表示机器人V_i的运动轨迹.编队运动过程中，边长轨迹定义为D(q)=‖q_i(t)-q_j(t)‖=l(l为常量且∀(V_i，V_j)∈E)，当编队过程中所有边长轨迹均保持恒定值，称对应的图为刚性图，否则为可变形图.

定义1  最小刚性图是指能够维持刚性且具有最少边数的一类图，在R²中，含有n(n≥2)个顶点的刚性图是最小刚性图，当且仅当其边数e=2n-3；在R³中，含有n(n≥3)个顶点的刚性图是最小刚性图，当且仅当其边数e=3n-6.

定义2  全局刚性是指根据给定的顶点间的位置约束，映射得到的刚性图中所有顶点间的位置关系是唯一的.

考虑刚性图和全局刚性图的区别，如指定4个顶点间的位置约束如式(1)所示：

(1)

根据上述位置约束，映射得到的刚性图有4种不同的结构框架，如图 1所示，其中图 1(a)和图 1(b)、图 1(c)和图 1(d)中，所有顶点分别满足相同的位置约束，即编队队形相同，看做同一种刚性图，且两种刚性图可以通过沿某条边翻转得到.

由图 1可知，根据给定的4个顶点间位置约束映射得到刚性图，顶点2和顶点4的位置关系是不唯一的，所以不是全局刚性图.

定理1  在R²中，含有n(n>3)个顶点的最小刚性图一定不满足全局刚性条件.

1.2 持久图及分类

持久图是有向图，即各边皆为有向边，有向边表示在运动过程中，顶点V_i保持与顶点V_j的位置约束.

定义3  在R^d(d∈{2，3})中，如果一个有向图中任意顶点的出度d⁺(i)≤d且其对应的无向图是刚性图，则该有向图称为持久图.

定义4  如果持久图对应的无向图是最小刚性图，则该有向图称为最小持久图.

定义5  有向图的结构持久特性指每个顶点保持对其的位置约束且在运动过程中所有顶点之间的位置关系维持不变，进而使编队队形得以保持.满足结构持久特性的持久图称为结构持久图.

考虑持久图和结构持久图的区别，如图 2所示的3维持久图，根据定义3及定义4可知，图 2(a)和图 2(b)为最小持久图. 图 2(a)是持久图但不是结构持久图，其中d⁺(1)=d⁺(2)=0，顶点1和顶点2的自由度为3，可以在3维空间任意移动不存在位置约束，即编队中存在两个领导者，而顶点3、顶点4和顶点5无法始终同时满足与顶点1和顶点2的位置约束，导致编队队形无法保持. 图 2(b)是结构持久图，d⁺(1)=1，需要满足与顶点3的位置约束，顶点2是编队中的唯一领导者，可以维持结构持久编队(下文简称持久编队).

定理2^[5]  2维空间的持久图都是结构持久图；3维空间中的持久图满足结构持久特性当且仅当在图中只有一个领导者.

2 基于视觉SLAM的机器人自主定位

机器人的位置信息对机器人的编队具有很大的影响，精准的位置能够使编队队形更易形成，队形能够更加持久.基于RGB-D相机的视觉SLAM算法，能够给机器人提供精准的位置信息，使机器人位置误差大大减小^[13].

视觉SLAM算法系统框架如图 3所示.

系统前端负责处理传感器数据后进行特征提取和特征匹配，然后构成视觉里程计.回环检测主要用于闭环存在时对系统进行优化.后端为优化部分，利用图优化理论对视觉里程计的累积误差进行优化，从而减小误差，最终得到全局一致轨迹，由此便可以得出机器人的精确位置信息.整个系统中，前端RGB-D相机全称为Red Green Blue-Depth相机，特征提取部分为提取ORB(Oriented FAST and Rotated BRIEF)特征，PnP即为Perspective-n-Point的缩写，是求解3D点到2D点对运动的方法. BA优化即为Bundle Adjustment，是一种常用的优化方法.

2.1 前端视觉里程计 2.1.1 特征提取及匹配

在每个图像中提取1 000个角点作为关键点，为确保角点在图像中均匀分布，将图像以网格作为刻度级别，每个网格至少提取5个角点.在每一个网格中检测角点，如果没有检测到足够的角点，则调整检测的阈值.最终在检测到的角点上提取ORB描述子^[16]及关键点的方向.利用提取的描述子及方向信息，采用快速近似最近邻^[19]的方法来对两帧图像进行特征匹配^[18].

2.1.2 关键帧的选取

相机位姿需要利用关键帧来进行求解，因此关键帧的选取对于系统的鲁棒性和稳定性有重要的影响.在该算法中，关键帧的选取遵循3个准则：

1) 连续未被选为关键帧的帧数超过20；

2) 当前帧中至少有50个关键点能够被其它关键帧追踪；

3) 当前帧与前面所有选出的关键帧不能够匹配.

通过以上的关键帧选取原则能够很大程度上地减小算法计算复杂度，提高系统的实时性.

2.1.3 机器人位姿估计

基于选取的关键帧来对机器人的位姿进行求解.首先对每个关键帧中的关键点进行RASANC迭代去除离群点，然后利用PnP算法来求解位姿.

假设空间某点坐标为P_i=[X_i，Y_i，Z_i]^T，深度值为s_i，其投影的像素坐标为U_i=[u_i，v_i]^T，像素位置与空间点位置的关系为

(2)

其中，K为相机的内参数矩阵，ξ为相机位姿的李代数表示，^表示向量生成反对称矩阵.将其转换为矩阵的表示形式为

(3)

此处包含着向量的归一化处理，在视觉SLAM中，为了方便表示，通常归一化处理之后都用原符号来表示.由于相机位姿是未知的且在观测点处存在噪声，因此该等式存在一个误差.该误差是像素坐标与3D点按照当前估计的位姿进行投影得到的位置相比较得到的，称之为重投影误差.将该误差求和，构建最小二乘问题，然后使其最小化求得最优的位姿解.

(4)

最后利用g2o(general graphic optimization)中的列文伯格—马夸尔特算法^[17]来对其进行求解. g2o是一个在SLAM领域广为使用的优化库，是一个基于图优化的库，提供了大量的顶点和边的类型，非常便于求解相机位姿估计问题.

2.1.4 局部位姿优化

在本文中，利用光束法平差(bundle adjustment)来对局部几个关键帧求解的位姿进行优化，通过最小化已匹配的世界坐标系下的3D点Xⁱ∈R³和关键点X_sⁱ∈R³的重投影误差，来优化相机的旋转矩阵R∈SO(3)(即R为李群中的3维旋转群)和平移t∈R³.优化方程为

(5)

其中，ρ表示Huber损失函数，Σ表示与关键点相关联的协方差矩阵，π_s定义为

(6)

其中，f_x、f_y、c_x、c_y、b为相机内参，都可以通过相机标定得到.

优化过程中使用关键帧集合k_L和这些关键帧所能观测到的关键点集合p_L.其它的能够观测到p_L中的点但不属于k_L的关键帧k_F，也会对损失函数产生一定的影响，因此需要对式(5)进行改进.设x_k为关键帧集合k_F中的某一帧观测到的p_L中的点，则最终的优化函数为

(7)

(8)

通过以上局部位姿优化，可以得到更加精确的机器人位姿.

2.2 回环检测

回环检测主要解决位置估计随时间漂移的问题，通过判断图像之间的相似性来完成回环检测，其实质上是一种计算图像数据相似性的算法.如果回环检测成功，可以显著减小累积误差.本文中，采用基于词袋模型BoW(bag of words)^[15]的方法来进行回环检测.

2.2.1 建立词袋库

建立词袋库的方法为将所有图像中的N个特征点，构建一个深度为d，每次分叉为k的树，具体步骤为：

1) 在根节点利用K均值方法将所有样本聚成k类，得到第1层；

2) 将属于第1层各个节点的样本分别再聚成k类，得到下一层；

3) 以此类推，最终得到d层，即叶子层.叶子层即为所谓的Words.

2.2.2 相似度计算

在得到字典之后，针对于其中的某幅图像，可以基于词典建立描述图像中含有的单词的向量，向量维度为词典所具有的单词数，然后利用该向量来计算两幅图像之间的相似度.

定义IDF(inverse document frequency)为：某个叶子节点ω_i中的特征数量相对于所有特征数量的比例.设所有特征数量为n，ω_i中的特征数量为n_i，则：

(9)

定义TF(term frequency)为：某个特征在单幅图像中出现的频率.假设A图像中的单词ω_i出现了m_i次，而ω_i出现的总次数为m，则：

(10)

单词ω_i的权重η_i为TF和IDF之积，即：

(11)

考虑权重后，描述图像A的向量可以写成：

(12)

v_A是一个稀疏的向量，维数取决于字典中单词的数量.其中会含有大量的0，非零部分指示了图像A中含有哪些单词.给定A、B两幅图像的描述向量v_A和v_B，计算它们相似度的公式为

(13)

得到图像间的相似分数之后，要对相似性评分进行归一化处理，取某时刻关键帧图像与上一时刻的关键帧的相似性成为先验相似度s(v_t，v_t-Δt)，所有分值都参照这个值进行归一化处理：

(14)

若当前帧与之前某关键帧的相似度超过当前帧与上一个关键帧相似度的3倍，则认为可能存在闭环.

2.2.3 检测验证

检测之后要对其进行验证以确保回环检测的正确性.检测方法为设立回环缓存机制，认为单词检测到的回环不足以构成良好的约束，而在一段时间中一直检测到的回环才认为是正确的回环.

2.3 后端图优化

SLAM算法前端计算出机器人位姿，构成位姿图的节点，位姿之间的转换关系构成位姿图的边，位姿图节点为优化变量.由于位姿的估计过程中存在误差，因此最终的位姿图的边不能形成一个全局一致的轨迹.将图优化的问题表示为最小化非线性误差函数的问题，非线性误差函数为

(15)

其中，表示机器人位姿向量；Z_ij表示位姿X_i和X_j之间约束的平均值；Ω_ij表示位姿X_i和X_j之间约束的信息矩阵；X_i和X_j是由前端计算出来的成对变换；e(X_i，X_j，Z_ij)表示矢量误差函数，用于表示位姿X_i和X_j之间约束满足Z_ij的程度，当约束满足Z_ij时矢量误差函数取值为0.在闭环存在的情况下，由于图中的循环闭合边缘能够减小累积误差，因此图优化能够更好地优化全局轨迹.

利用g2o工具来求解使得式(15)最小的机器人位姿，得到全局优化后的机器人位姿，即得到全局一致轨迹.由此便可以得到编队所需的机器人位置q_i=[x_i，y_i，0]或者无人机位置q_e=[x_e，y_e，z_e].

3 基于结构持久图的编队动态生成 3.1 维空间中最小持久编队动态生成

针对持久编队动态生成问题，首先介绍有向图中增加顶点的操作，如图 4(a)所示：V_i、V_j为最小持久图G=(V，)的两个顶点，有向图中增加顶点的操作是通过增加顶点V_k与有向边和，同时保持图的最小持久性不变.但在持久编队动态生成过程中，由于机器人初始位置不确定，在有向增加顶点操作过程中，图 4(a)和图 4(b)满足相同的距离约束，但由于不满足全局刚性而得到不同的编队队形，由此类推，执行m次该操作，由于初始位置不确定，虽然得到最小持久图，但不满足全局刚性，即可能产生2^m种编队队形，所以无法保证得到预定的编队队形，产生局部极小问题，甚至有可能发生碰撞.

考虑初始位置的影响，保证得到预期的编队队形，提出一种改进的有向增加顶点操作方法.

定义6  因为当顶点数目小于3时，根据距离约束条件，此时不存在不同的编队队形，所以本文定义扩展基础图为由3个顶点和3条有向边组成的最小持久图，在此基础上进行持久图扩展.

由有向增加顶点操作得到的不同编队队形可以理解为由给定位置约束映射得到的不同的刚性图，如图 1所示.若按照图 5进行扩展，则能够满足全局刚性，但根据定义3，在R²中，持久图中任意顶点出度d⁺(i)≤2，而d⁺(k)=3，所以不满足持久特性.

定理3  以扩展基础图为基础的最小持久编队动态生成过程中，若满足以下的约束关系，则可以克服局部极小问题，得到唯一的最小持久编队队形(以图 4为例)：

1) 顶点V_k与顶点V_i(称为第1顶点)、顶点V_j(称为第2顶点)间的距离约束；

2) 顶点V_k与顶点V_l(称为第3顶点)间的位置约束，即根据预期队形需要在有向边V_ki的同侧或异侧.

证明  由图 1及其分析可知，当满足约束1)时可得到2种刚性编队队形，当满足约束2)时可得到确定的刚性编队队形.由图 4及其分析可知，该操作是有向增加顶点操作的一种特殊情况，具有更强的约束性，所以最小持久性可保持不变，所以满足上述约束条件可得到唯一最小持久编队队形.

3.2 无人机与机器人最小持久编队动态生成

由定理2可知，3维空间中持久图满足结构持久特性要求编队中只有一个领导者，同时考虑实际机器人只能在地面移动，所以选定任一地面机器人为领导者，而且无人机自由度必须为0，即无人机V_c出度d⁺(c)=3.

定理4^[12]  由2维空间中图形G扩展得到的3维空间中的锥形图G^*{c}，G^*{c}在3维空间满足全局刚性，当且仅当G在2维空间满足全局刚性.

如图 6所示，以扩展基础图作为2维空间中的图形G，通过增加顶点V_c，得到3维空间中的锥形图G^*{c}.考虑实际编队过程中约束条件，顶点V_c的出度d⁺(c)=3.

根据定义3，经过有向增加顶点操作得到的有向图为持久图且能够保持最小持久特性.同时，扩展基础图能够满足全局刚性，并且由定理3可知，得到的最小持久图能够满足全局刚性，即可得到唯一最小持久编队队形.

综合上述理论，给出基于结构持久图的最小持久编队动态生成算法，算法程序流程图如图 7所示.

1) 输入：编队结构G^*{c}=(V^*，E^*)，顶点集合V^*={V₁，V₂，…，V_n}∪{V_c}(n个机器人和1个无人机).

2) 算法操作：

(1) 计算邻接矩阵A及每个顶点的出度；

(2) 指定领导机器人V_l、第1跟随机器人V_j和第2跟随机器人V_i构成初始扩展基础图.

(3) 将初始扩展基础图的3个顶点出度置为2.

(4) 基于改进有向增加顶点操作(设新增顶点与第3顶点异侧)获取机器人最小持久编队队形.

(5) 基于有向增加顶点操作结合无人机V_c构造最小持久编队：结合扩展基础图顶点建立有向边得到最小持久编队.

4 基于势场函数的队形控制 4.1 势场函数

定义7^[19]  约束邻接机器人V_i和V_j相对距离的势场函数为V_ij(q_i)，其中q_i表示V_i的坐标位置，针对结构持久图定义，满足V_i相对于V_j的距离约束的势场函数定义为

(16)

式中，α为正比例系数，d_ij=‖q_i-q_j‖为机器人V_i与机器人V_j之间的实际距离，l_ij为机器人V_i与机器人V_j之间的设定期望距离.

由式(2)可知，势场函数V_ij(q_i)具有特点：

1) 当d_ij→∞时，V_ij(q_i)→∞；

2) 当d_ij=l_ij时，V_ij(q_i)取最小值.

势场力为势场函数的梯度函数，定义为

(17)

其中，为q_i指向q_j的单位向量，其余参数同式(16).

由式(17)可知，势场力函数具有特点：

1) 当d_ij < l_ij时，，表示机器人V_i受到机器人V_j的斥力作用；

2) 当d_ij>l_ij时，，表示机器人V_i受到机器人V_j的引力作用；

3) 当d_ij=l_ij时，，表示机器人V_i与机器人V_j满足相对距离约束.

4.2 地面机器人队形控制 4.2.1 问题描述

双轮机器人的运动具有非完整性，只有x轴方向上的线速度和z轴方向上的角速度，满足运动学方程：

(18)

其中，v_i表示机器人V_i前行的线速度，ω_i表示机器人转动的角速度，θ_i表示机器人的朝向.

定义8  虚拟机器人初始状态由相对应的机器人V_i的位置坐标q_i=[x_i，y_i]及领航者机器人V_l的朝向θ_l决定，即虚拟机器人的初始坐标且朝向.虚拟机器人满足1阶积分模型：

(19)

由于双轮机器人的非完整性，不能像理想积分模型一样沿势场力方向运动，根据定义8构造虚拟机器人，把双轮机器人编队控制问题分解为：

1) 利用势场力函数设计编队控制器控制虚拟机器人生成指定队形；

2) 基于李亚普诺夫方法设计控制器实现机器人V_i对相应的虚拟机器人的轨迹跟踪.

控制结构图如图 8所示.

4.2.2 地面机器人势场力函数设计

定理5  针对N个机器人编队问题，若机器人V_i能够实现对相应的虚拟机器人轨迹的跟踪，则在式(17)的势场力函数作用下，编队系统稳定.

证明  虚拟机器人总的结构势函数为

(20)

其中，为虚拟机器人的有向邻居集，其余参数由式(16)可知.

选择李亚普诺夫函数，则有.对于虚拟机器人，结合式(19)可得：

(21)

则：

从而满足.由李亚普诺夫稳定性理论得，在控制律(21)作用下编队系统稳定.

4.2.3 扩展基础图中机器人势场力函数设计

假设领航机器人为V_l，第1跟随者为虚拟机器人，第2跟随者为虚拟机器人(如图 6(a)所示). 仅需满足与V_l的距离约束，即仅需满足‖q_i(t)-q_l(t)‖收敛到l_il；需同时满足与V_l及间距离约束，即需满足‖q_j(t)-q_l(t)‖收敛到l_jl，同时‖q_j(t)-q_i(t)‖收敛到l_ji.所以，针对虚拟机器人、虚拟机器人分别设计势场力函数，如式(8)和式(9)所示：

(22)

(23)

式中相关参数解释及物理意义同式(17).由定理5可知，在上述势场力函数作用下，编队系统稳定.

4.2.4 普通机器人势场力函数设计

假设领航机器人V_l、虚拟机器人及虚拟机器人构成扩展基础图，结合最小持久编队动态生成算法生成最小持久编队.以图 4(a)所示扩展持久图(增加虚拟机器人)为例，设计虚拟机器人的势场力函数，如式(24)所示：

(24)

式中，前两项相关参数及物理意义同式(17).若预期队形中新增顶点与第3顶点异侧，则：

若预期队形中新增顶点与第3顶点同侧，则：

式中，第3顶点的物理意义为，通过引入附加作用力，避免由于不满足全局刚性导致的局部极小问题，即满足改进的有向增加顶点操作.由定理5可知在上述势场力函数作用下，编队系统稳定.

4.2.5 地面机器人控制器设计

利用4.2.2节所述方法，得到由虚拟机器人构成的期望队形，本节利用李亚普诺夫方法设计控制器实现机器人V_i对相应虚拟机器人的轨迹跟踪，即可控制多机器人生成并保持期望队形.

机器人V_i在跟踪虚拟机器人过程中的运动模型为式(25)，其中机器人V_i的状态为(x_i，y_i，θ_i)，虚拟机器人的状态为.

(25)

式中，为距离误差，θ=α-θ_i，为角度误差，通过设计控制器同时满足ρ→0，θ→0即可实现对虚拟机器人的位置跟踪.

当机器人到达指定位置，调整朝向，使θ_i→θ_l，实现对虚拟机器人的状态跟踪，从而形成并保持编队队形.

定理6  设计控制器：

(26)

其中，k_v、k_e为正比例系数.在该控制器作用下，系统一致渐近稳定于平衡状态ρ=0，θ=0.

证明  选择系统李亚普诺夫函数：

则：

(27)

将式(26)代入式(27)得，所以系统在控制律(26)作用下一致渐近稳定.

4.3 无人机势场力函数设计

考虑无人机满足1阶积分模型：

(28)

式中，q_c=[x_c，y_c，z_c]为无人机位置坐标.

由定理5可知，2维空间中，1阶积分模型在控制律(21)作用下系统稳定.同理，将势场函数及势场力函数定义扩展到3维空间，在控制律(29)作用下系统稳定.

(29)

式中，α为正比例系数，d_cj=‖q_c-q_j‖(q_c=[x_c，y_c，z_c]，q_j=[x_j，y_j，0])为无人机V_c与机器人V_j之间的实际距离，l_cj为无人机V_c与机器人V_j之间的设定期望距离.

由定理2可知，为保持结构持久特性，无人机V_c的出度为3，所以结合扩展基础图中机器人位置信息构造如式(30)所示势场力函数.

(30)

式中，α、β、γ为正比例系数，d_cl、d_cj、d_ci分别为无人机V_c与扩展基础图中3个机器人的实际距离，l_cl、l_cj、l_ci分别为无人机V_c与扩展基础图中3个机器人的期望距离.

5 实验验证

利用Gazebo仿真平台搭建实验环境.由4台轮式机器人turtlebot及四旋翼无人机组成，其中turtlebot机器人和无人机搭载Kinect RGB-D相机，利用视觉SLAM算法反馈实时位置信息；利用开源机器人操作系统ROS编程实现多机器人的编队控制.

5.1 视觉SLAM算法定位实验 5.1.1 TUM数据集

首先在TUM RGB-D数据集^[21]上评估该视觉算法. TUM RGB-D数据集提供了一个包含RGB-D数据和地面实况数据的大型数据集，本文选择其中的freiburg1_room数据集，该数据集的持续时间为48.90 s，地面实况的轨迹长度为15.989 m，相机运动速度为0.334 m/s.在该数据集上运行本文中提出的定位算法，将运行过程中的相机轨迹数据存储下来并与数据集中提供的地面实况数据进行对比，求得一系列的绝对平移误差值.最终对比的结果如表 1所示.由表中的数据可以得出，平均误差值为9.6 cm，误差的最大值为15.377 cm，由此可以得出本文提出的定位算法能够对机器人进行精准定位.

5.1.2 视觉SLAM算法仿真验证

在Gazebo仿真平台中，加载小型四旋翼模型搭载Realsense摄像头，无人机的飞行轨迹设为半径为3 m的圆形，飞行高度设为2 m，飞行速度设为3 m/s，利用该仿真平台来验证视觉SLAM算法. 图 9为Gazebo下的无人机仿真环境，图 10为无人机在该环境下飞行一圈后的实验结果，其中绿色方块为无人机的当前位置q_c=[x_c，y_c，z_c]，蓝色方块为无人机飞行途中的位置，黑色点和红色点为定位算法提取出的ORB特征点，绿色线为无人机的飞行轨迹.由仿真结果结合5.1.1节的验证结果可以得出，该定位算法能够对无人机进行精准定位.

5.2 编队队形生成及保持

基于Gazebo仿真平台，搭建多Turtlebot机器人模型和四旋翼无人机模型，其中Turtlebot的控制量是线速度和角速度，四旋翼无人机控制量是速度，无人机及多机器人通过视觉SLAM算法进行自主定位.如图 11所示拓扑结构，设计1架无人机与4台机器人的编队协同实验，其中1号机器人为领航机器人，2号机器人为第1跟随者，3号机器人为第2跟随者，4号机器人为普通跟随者，分别根据式(22)~式(24)设计势场函数，并通过式(26)设计控制器，无人机视作理想积分模型，根据式(30)设计势场函数.

无人机与多机器人初始位置如图 12(a)所示，设定各顶点间间距为4 m，编队队形形成过程及最终形成的编队队形如图 12(b)、图 12(c)、图 12(d)所示.编队队形生成过程中，多机器人实时距离曲线及多机器人速度信息分别如图 13所示.由该结果可以得出，本文中所提基于结构持久图的编队生成算法能够有效的使得不同平面下的多智能体形成编队队形.

给定领航机器人线速度阈值为0~0.6 m/s，角速度阈值为0~0.5 rad/s，并以5 m为半径做圆周运动，编队队形保持效果如图 14所示.在编队队形生成的基础上，本文所提的控制理论能够对多智能体进行控制，使其很好地保持队形，验证了所提理论的有效性.

5.3 持久编队队形变换

根据环境地形改变编队队形拓扑结构，调整多机器人间间距，使编队整体顺利通过走廊，如图 15所示.该结果表明本文中所提的理论能够在多智能体所处环境发生改变时，顺利地改变编队队形拓扑结构，从而适应不同的环境，并能做到对其进行精确的控制.

6 结论与展望

本文提出了一种基于结构持久图的多机器人最小持久编队生成与控制方法.算法基于RGB-D相机的视觉SLAM算法提供精确的位置信息，并利用势场函数分别设计多机器人的编队控制器.实验中针对多机器人的最小持久编队生成和持久运行效果，利用Gazebo仿真平台对无人机与多机器人最小持久编队及视觉SLAM算法定位进行了验证.实验结果表明，该方法在降低通信复杂程度的同时可以有效且持久维持刚性队形，具有较高的可靠性.

目前，本文只针对一架无人机与多机器人的持久编队生成与控制问题进行了研究.后续研究中，首先考虑多无人机与多机器人的协同编队问题，同时考虑编队运行中的干扰问题.