基于VICON的无人机编队分布式控制

Distributed Control of UAVs Formation Based on VICON

摘要: 无人机编队执行任务相比单架无人机有着成功率高和任务量大的优点，编队飞行控制器的设计和编队通信方式是自主编队飞行的关键。本文针对无人机自主编队飞行的控制及通信问题，在室内动作捕捉系统VICON环境下，设计了编队轨迹跟踪以及队形保持控制器实现无人机编队巡航；采用基于代数图论和智能决策的方法，每架无人机都拥有判断自身编队层次的能力，即使有无人机出现故障，也能通过将编队信息流再次连通实现编队重构。同时利用模块化的通信设备，方便了通信程序的修改。编队飞行实验结果验证了本文提出方法的有效性。

Abstract: Comparing with single unmanned aerial vehicle (UAV), the UAVs formation has many advantages, such as high success rate and multi-task. The design of controller and the formation communication mode of UAVs are the key techniques to autonomous formation flight. This paper focuses on the control and communication of autonomous formation flying for UAVs. Under the environment of the indoor action capture system, which is made in VICON UK so it is called VICON system, the formation trajectory tracking and formation keeping controller are designed to realize the UAVs formation cruising. By using algebraic graph theory and intelligent decision-making method, each UAV has the ability to judge the self-position and formation information. Moreover, if there is a malfunction of the UAV, it can also reconnect formation information to realize the formation reconfiguration. At the same time, using modular communication module, it is convenient to modify communication program. The results of the formation flight experiment verify the effectiveness of the method in this paper.

1. 引言

随着科学技术的发展，无人机不管是在军用还是民用领域都发挥着重要的作用 [1] [2] ，单架无人机在执行任务时一旦出现故障就意味着任务失败，但是对于无人机群来说，一架无人机出现故障离队后，利用动态网络感知技术获取环境变化和机群内部编队发生的变化，其余的无人机通过保持通信网络信息流为连通图来变换机群的编队队形和通信网络，这样就能继续执行任务 [3] 。

无人机编队飞行协同执行任务就是将有自主功能的无人机根据要求按照一定的三维空间结构排列飞行并能根据控制命令动态调整姿态，保持机群飞行过程中的队形相对稳定。编队飞行能够发挥单机的最大作战效能，使无人机更安全、可靠、高效地完成任务 [4] 。编队控制主要有虚拟领航者法 [5] 、人工势场法 [6] [7] 、领航–跟随者法 [8] 等。虚拟领航者法用虚拟的无人机担任领航者，无需担心领航者出现故障的问题，但需要很高的计算能力，同时虚拟领航者不是实际的飞行路线，无法保证跟随者能够准确跟随虚拟领航者；人工势场法虽然在数学描述上简单又美观，但是很难保持稳定的队形 [9] ；领航–跟随者法是目前编队最可靠的方法之一，跟随者保持与领航者相对距离和方位来维持编队队形，领航者的角色十分重要，一旦出现故障可能会导致任务的失败。

本文采用领航–跟随者法，设计了轨迹跟踪控制器和队形保持控制器，同时针对无人机编队飞行过程中无人机出现故障的问题，设计了一种基于图论的领航者重选的编队重构控制方法，图论能够很清晰直观的描述无人机编队队形与通信信息流，用图中的顶点集表示无人机群，图中的边集表示通信信息流。在通信中考虑了无人机本能的问题，每一架无人机都有判断自身所处编队层次的能力，顶层的为领航者，越上层的无人机越为重要，当有无人机出现故障，可以通过判断该无人机在图中对应的顶点是否为割点，以此决定是否会对机群其它无人机产生影响，若是领航者出现故障，将次顶层的无人机重新选为领航者，解决了初始的领航–跟随者法领航者出现故障导致任务失败的问题。

2. 无人机编队控制器

本文采用的是基于领航–跟随者方式编队，编队飞行控制系统包括队形保持控制子系统和轨迹跟踪控制子系统。领航者的任务在于能够精确的跟踪期望轨迹，因此需要对领航者设计轨迹跟踪控制器；另外，根据需要保持的队形，利用领航者和跟随者之间的相对运动方程，设计队形保持控制器，在此过程中跟随者可以获得保持队形所需要的期望位置，为了使跟随者能够更快的跟上领航者，在队形保持控制器中加入了误差混合器，既保持了编队队形，又提高了跟随者对领航者的快速跟随，编队控制器结构如图1所示。

Figure 1. Formation controller structure

图1. 编队控制器结构

在编队控制器中，误差混合器是由无人机在导航坐标系的位置误差、领航者与跟随者的速度误差线性合成的，数学表达式为：

$\begin{array}{l}{e}_{vx}=k_x\left(x_{f0}+x_l-x_f\right)+k_{vx}\left(v_{lx}-v_{fx}\right)\\ e_{vy}=k_y\left(y_{f0}+y_l-y_f\right)+k_{vy}\left(v_{ly}-v_{fy}\right)\\ e_z=k_z\left(z_{f0}+z_l-z_f\right)\end{array}$ (4)

式中， $k_x,k_{vx},k_y,k_{vy},k_z$ 为误差线性混合器的权重参数， $x_l,y_l,z_l$ 为领航者期望位置， $x_f,y_f,z_f$ 为跟随者的实际位置， $v_{lx},v_{ly}$ 为领航者的实际速度， $v_{fx},v_{fy}$ 为跟随者的实际速度。

采用PID控制律，得到的是跟随者的加速度信息，数学表达式为：

$\begin{array}{l}{a}_{fx}=k_{px}{e}_{vx}+k_{ix}{\displaystyle {\int }_0^t{e}_{vx}\text{d}t}+k_{dx}{\stackrel{\dot{}}{e}}_{vx}\\ a_{fy}=k_{py}{e}_{vy}+k_{iy}{\displaystyle {\int }_0^t{e}_{vy}\text{d}t}+k_{dy}{\stackrel{\dot{}}{e}}_{vy}\\ a_{fz}=k_{pz}{e}_z+k_{iz}{\displaystyle {\int }_0^t{e}_z\text{d}t}+k_{dz}{\stackrel{\dot{}}{e}}_{vz}\end{array}$ (5)

式中， $k_{px},k_{ix},k_{dx}$ 为x方向的比例、微分、积分系数， $k_{py},k_{iy},k_{dy}$ 为y方向的比例、微分、积分系数， $k_{pz},k_{iz},k_{dz}$ 为高度的比例、微分、积分系数。

$x,y$ 方向加速度经过坐标变换到机体坐标，再通过姿态反解得到参考姿态角输入到姿态控制器。

3. 编队策略

图是由顶点集合和顶点间的二元关系集合组成的数据结构 [10] [11] [12] [13] ，通常用 $G\left(V,E\right)$ 来表示。顶点集合用 $V\left(G\right)$ 来表示，边的集合用 $E\left(G\right)$ 来表示。编队队形 $G_1\left(V,E\right)$ 如图2所示，顶点1为领航者，其余为跟随者，所有的无人机可表示为：

$V\left(G_1\right)=\left\{1,2,3,4,5,6,7\right\}$ (6)

通信信息流为：

Figure 2. Formation information flow chart

图2. 编队信息流图

$E\left(G_1\right)=\left\{\langle 1,2\rangle ,\langle 1,3\rangle ,\langle 1,4\rangle ,\langle 1,5\rangle ,\langle 2,6\rangle ,\langle 2,7\rangle \right\}$ (7)

其中，箭头方向表示上一级的无人机向下一级的无人机发送控制指令信号，回馈信号在图中省略。要保持编队队形，则必须使编队信息流图为连通图，即去除箭头后图中任意一对顶点都是连通的。若领航者出现故障，如图4(a)所示，此时应该重新选择领航者，将最高层次的跟随者改变工作状态在领航者模式，其余领航者则不变。

本文中无人机只与一架层次高于自身的无人机通信，但可以与一架或多架层次低于自身的无人机通信，因此可以利用图论中的深度优先搜索(Depth First Search, DFS)法找到图中的关节点或割点，割点是图中的一个顶点，当去掉这个点会使图分割成2个或2个以上的连通分量，这就意味着当割点处的无人机出现故障时会破坏编队通信图的连通性，机群其它的无人机会受到影响。当出现故障的不是割点处的无人机，不会破坏编队通信图的连通性，可以忽略该无人机；当出现故障的是割点处的无人机，若不是领航者，则将该点上下等级的无人机连通，若是领航者出现故障就需要重新选定领航者。重新选定的领航者最好是割点处的无人机，这样可以减少通信信息量。当只有一个割点时，则选该顶点对应的无人机重新选定为领航者，当有多个割点时，则应选择剩余连通分量多的顶点为新的领航者，若多个割点处于同一层次，则选择最小ID的无人机为新的领航者。编队重构具体流程如图3所示。

综上，领航者智能重选策略可归纳为如下过程：

1) 无人机起飞，跟随者朝着领航者靠近并维持编队队形悬停，根据无人机的队形和通信信息流图构建相应的邻接矩阵，利用DFS法找到图中所有的割点，等待新的飞行命令。

2)根据每架无人机返回的ID判断是否有无人机出现故障。

3) 若所有无人机工作正常或是非割点处的无人机出现故障，跳至第5步；若割点处的无人机出现故障，判断是否为领航者。

4) 若不是领航者出现故障，将故障无人机低层次的无人机与高层次的无人机连接通信，同时更新邻接矩阵；若是领航者出现故障，则按照规则重新选定领航者并更新邻接矩阵。

5) 领航者根据预先设定的飞行线路进行轨迹跟踪控制飞行，跟随者根据领航者发来的位置与速度信息进行编队控制飞行。返回第2步。

图2中，顶点1和顶点2为割点，1号领航者出现故障，只剩下2号割点，将2号无人机选定为领航者，将剩下的连通分量与领航者实行通信连通，重新选定领航者后的编队队形如图4(b)所示，且新的编队通信无向图为连通图。

编队飞行通信程序代码量比较大 [14] [15] ，在原有飞行控制器上修改通信程序后还需要重新对整个程序进行编译及下载固件，这会浪费大量的时间，本文开发了模块化的通信设备，只需对通信模块进行修改再连接至飞行控制器即可，极大的方便了通信程序的修改，通信模块如图5所示。

4. 实验验证

为了验证控制器和通信的有效性，在VICON光学运动捕捉系统(简称VICON)的环境下进行编队飞行实验，VICON可以通过安装在四周的红外摄像头捕捉对象，并在其构建的坐标系下获取对象的位置和速度等信息，具有精度高，采样频率快等优点，VICON及其相关软件如图6所示。

在室内VICON环境的坐标系下预先设定编队飞行轨迹与编队队形，由于室内环境空间有限，VICON环境下的坐标系为5 m × 5 m的空间，编队飞行轨迹为直线，编队队形为三角形，具体如图7所示。

三机编队飞行轨迹如图8所示。

从图8可以看出，在室内VICON环境下，领航者可以较好的跟踪设定轨迹，跟随者也能迅速的跟随领航者，从而保持编队队形。

Figure 3. Flow chart of formation reconfiguration

图3. 编队重构流程图

(a) (b)

Figure 4. Contrastive map before and after formation reconfiguration

图4. 编队重构前后对比图

为了验证无人机群在有无人机出现故障后编队队形是否能够保持，实验中将模拟领航者出现故障，程序中设定机群在巡航飞行的过程中领航者突然从自动模式切换至降落模式，此时串口不再向下一级跟

Figure 5. Communication data processing module

图5. 通信数据处理模块

Figure 6. VICON and upper computer

图6. VICON与上位机

Figure 7. Formation map

图7. 编队队形图

Figure 8. Formation flight trajectory map

图8. 编队飞行轨迹图

Figure 9. Formation reconfiguration trajectory map

图9. 编队重构轨迹图

随者发送参考量，领航者降落后自动上锁不再飞行，剩余无人机因为接收不到信号先在当前地点悬停，根据优先级重新选择领航者，VICON上位机继续发送参考轨迹至新的领航者，并将新领航者的ID发送至跟随者，跟随者通过ID连接至跟随者从而保持通信网络的连通继续编队飞行，编队重构实验结果如图9所示。

从图9可以看出，本文中轨迹跟踪控制器能够使领航者按照预定的路径飞行，编队控制器能够保持机群编队队形，智能通信控制能够有效地维持机群中无人机出现故障时通信图的连通性。

5. 结论

针对领航–跟随者编队方法的无人机自主编队飞行控制的问题，设计了轨迹跟踪控制器以及队形保持控制器，在编队控制器中加入误差混合器，使跟随者不仅能够快速的跟随领航者的机动变化，还能有效的保持编队队形。同时在机群通信中采用了基于代数图论的智能决策方法，解决了领航者或者其它无人机出现故障导致飞行任务失败的问题。实验结果表明编队控制器能够很好的实现编队巡航飞行，并且在故障情况下也能实现编队重构。

基金项目

国家自然科学基金面上项目(No. 61573186)资助。

参考文献