CN110501898A - 一种应用在无人机上的双机群控策略 - Google Patents

Abstract

本发明公开一种应用在无人机上的双机群控策略，随着科学技术的发展，越来越多的科研人员将目光关注到计算机算法上，一个优秀的算法可以很大程度上提高计算机的处理效率。在控制学领域，越来越多的优化算法在控制领域发挥了重要的作用。无人机由于其独特的模型，受到大量科研人员和航模爱好者的追捧与研究，近年来，无人机的集群控制越来越受到研究人员的重视，我们根据经典的四旋翼无人机的数学模型，采用了双闭环PD算法设计主从双机无人机集群控制系统，用Simulink进行了仿真，由于双闭环PD系统的良好的鲁棒性，克服了无人机的欠驱动特性，在仿真过程中，发现对于位置和姿态的控制有着良好的稳定性和动态特性。并将此算法应用于实际，有着良好的收益。

Description

一种应用在无人机上的双机群控策略

技术领域

本发明属于计算机技术与无人机结合的技术领域，特别是涉及一种应用在无人机上的双机群控策略。

背景技术

无人机是人们通过无线通讯设备和自动控制装置通讯控制不载人的飞机。英文简称 UAV，最早在20世纪20年代出现，因其具备的独特功能性和世界科学技术的发展，无人机被广泛应用。

无人机也被称作无人驾驶机器人，二战期间，无人机在应用于军事打击时取得了很重要的作用后，大量的研究人员和爱好者投入到无人机的研究中。在军事领域中，无人机的应用于军事打击、目标跟踪、侦查等，在民用领域中无人机被大量爱好者用于航拍，近年来国内外也开始应用无人机进行灭火、植保等。在无人机不断发展不断普及的情况下，美国、中国等国家相继制定诸多有关无人机的政策，例如无人机的实名登记政策等。今天在公园上空嗡嗡作响，对飞机造成干扰的四轴无人机其实早在1962年就申请了专利。 PiaseckiAircraft Corporation的工程师Edward G.Vanderlip最先设计了让直升机仪器在动力失效的情况下仍能继续工作的手段。当今社会越来越多的科技公司投入到无人机的研究中，无人机的集群控制更是今年来无人机领域的研究热点。国外的学者和专家对于无人机的研究以已有100多年之久，尤其是美国和以色列。其中内华达大学的专家，开发一架可以帮助患者行走的无人机。国内的大疆公司、小米公司等多家科技公司都已上市众多无人机产品。清华大学、北京航空航天大学等诸多高校也正对无人机做许多重点技术的研究。在集群控制的不断发展下，人们也渐渐将无人机表演当做一种环保且新奇的娱乐方式。由于无人机自身的特性，许多高校的学生都会选择用无人机作为研究对象，这也大大推动了无人机的发展。但无人机并不仅仅局限于现有，现今世界对无人机的研究仅仅只是一个开始，随着人类数量的增长，科技的发展，地球上可用的移动空间越来越小，无人机凭借其体型小且灵活的优势，可以在未来的生活中担任城市侦查、物流等任务。且就像无人汽车的发展一样，在未来的某一天，我们也终会乘坐无人驾驶飞机去旅行。

由于无人机的不断发展和普及，越来越多的无人机爱好者不断开发拓展无人机的功能，无人机集群控制是近年来新兴的科技产业，无人机集群控制是通过群控算法在各个无人机相互通讯的情况下，由两个或多个无人机针对任务要求，保持某种姿态队形进行飞行，通过算法控制多架无人机协同飞行的技术。基于集群控制的无人机机群有着诸多的优势，包括：飞行耗能低，任务执行率高，高效率等。由于无人机飞行算法的不断完善和小型民用无人机的发展，越来越多的人开始研究无人机用于舞台灯光表演。

发明内容

本发明所要解决的技术问题是，设计一种应用在无人机上的双机群控策略，采用了双闭环PD算法设计主从双机无人机集群控制系统，用matlab进行了仿真，由于双闭环 PD系统的良好的鲁棒性，克服了无人机的欠驱动特性，在仿真过程中，发现对于位置和姿态的控制有着良好的稳定性和动态特性。并将此算法应用于实际，有着良好的收益。

本发明提供的无人机上的双机群控策略包括：

1.旋转矩阵的推导：

通过旋转矩阵建立无人机的姿态变化模型，采用[φ，θ，ψ]表示无人机的三个姿态的欧拉角度，分别代表翻滚角、俯仰角和偏航角。在二维平面中，如附图2所示在XOY平面中有一向量旋转φ度后变为

根据附图可得：旋转φ度后

写成矩阵形式：

根据上述推导建立下列三维直角坐标系：

如附图所示，以坐标系的三个坐标轴X，Y，Z分别作为旋转轴，则点实际上只在垂直坐标轴的平面上进行二维旋转。假设绕X轴旋转φ度，那么：

旋转前：

旋转后：

则绕X轴旋转φ的旋转矩阵为

同理可得Y轴、Z轴的旋转矩阵

2.无人机的建模

由上述旋转矩阵解出无人机完整的旋转矩阵：

设无人机所受合力为F，其中包含无人机所受重力和四个电机为其提供的与重力相反的升力F1～F4。所以根据牛二律：

令u1为高度z的控制规律

u₁＝F₁+F₂+F₃+F₄ (11)

那么

将其展开，并加入空气阻力等其他扰动：

3.位置控制器的设计

由于四旋翼无人机具有六个自由度，但是无人机只有四个驱动电机，所以四旋翼无人机的控制变量较少，四旋翼无人机具有不可避免的欠驱动特性，所以我们无法对所有的自由度进行测量控制。在本文的研究中，我们选择跟踪无人机的位置和翻滚角，凭借位置和翻滚角的稳定控制来保证另外两个角度。

假设在集群控制的主从系统中，主机位置确定，地面站向所有从机发送主机位置，并要求所有从机的位置距主机达到一个指定距离。我们假设控制目标：

首先我们设计四旋翼无人机的位置控制系统：

根据式(18)，我们定义：

那么式(18)变成：

针对X轴方向的位置系统，我们根据上述PID公式设计PD控制器。

其中k_px为PID比例部分的参数，k_dx为PID微分部分参数，x为位置误差，为位置误差的微分，下同。(由于我们设定x→0，所以误差为x-0＝x)那么：

因为控制规律为二阶连续系统，我们可以使用劳斯判据或者用赫尔维兹稳定判据，鉴于后者计算比较方便，我们采用后者的稳定型判据来确定系统参数即：已知一个二系统： a₂s²+a₁s+a₀＝0根据赫尔维兹稳定判据可得，当系统稳定时a₀＞0，a₁＞0，a₂＞0，a₁a₀＞0那么公式(22)的参数范围为：k_px＞0，k_dx+K₁/m＞0我们取k_px＝5，k_dx＝5。同理对于第二个位置系统u_2x，我们同样设计PD控制系统其中

根据赫尔维兹稳定判据可得，当系统稳定时，k_py＞0，k_dy+K₂/m＞0我们取k_py＝5，k_dy＝5。第三个位置系统是竖直方向的位置控制系统，由于无人机受到重力影响，我们设计通过前馈控制的重力补偿PD控制系统：

这里的z_e是三维坐标系统Z轴上位置分量的误差。z_e＝z-z_d其中z_d是期望值。那么

同样根据赫尔维兹稳定判据可得，当系统稳定时，k_pz＞0，我们取k_pz＝5，k_dz＝5；

4.姿态控制器的设计

为了满足上述位置控制器所要求的姿态角度θ_d和ψ_d(这里的角度是期望值)需要实际角对于期望角度进行跟踪。根据上式可得：

由于上式可以变为：

因为u_1z＝u₁cosφcosψ且所以

我们可以得到：

我们可以得到：

这里如果公式(25)左边的绝对值超出1，那么θ_d将不存在，在实践中我们采用将等式左边的值用MATLAB函数进行归一化，但是会造成系统的姿态角度真实值失真，所以这里我们采用一种简单的限幅算法：

我们取当X＞1时，我们取sinθ_d＝1即当X＜-1时，我们取sinθ_d＝-1即当|X|≤1时，有sinθ_d＝X那么：

此时为了使公式(25)成立，我们可以将其写为

这里ε_1x，ε_1y，ε_1z为不定的实数值，它的大小根据X的变化而变化，例如当|X|≤1时ε＝0。ε相当于是加在控制器上的扰动，目的是为了保证公式(26)的成立，而它对系统稳定性的影响，将由控制器自身去克服。在求解θ_d和ψ_d后我们便得到位置控制规律为：

已知无人机的姿态系统数学建模为：

定义θ_e＝θ-θ_d，设计前馈补偿的PD控制算法：

则从而根据赫尔维兹稳定判据可得，当系统稳定时，k_p4＞0，我们取k_p4＝15，k_d4＝15 取ψ_e＝ψ-ψ_d，针对第二个姿态角：

那么从而根据赫尔维兹稳定判据可得，当系统稳定时k_p5＞0，我们取k_p5＝15，k_d5＝15。

定义φ_e＝φ-φ_d，所以对第三个姿态角：

根据赫尔维兹稳定判据可得，当系统稳定时k_p6＞0，我们取k_p6＝15，k_d6＝15

附图说明

图1二维平面中向量的变化示意图

图2三维坐标系中的变化

图3整个系统的框架

图4仿真的主程序

图5仿真的结果1

图6仿真的结果2

图7仿真的结果3

图8误差信号的变化

图9仿真的结果4

图10仿真的结果5

图11仿真的结果6

具体实施方式

实例1

1.旋转矩阵的推导：

通过旋转矩阵建立无人机的姿态变化模型，采用[φ，θ，ψ]表示无人机的三个姿态的欧拉角度，分别代表翻滚角、俯仰角和偏航角。在二维平面中，如附图2所示所示在XOY 平面中有一向量旋转φ度后变为

根据附图可得：旋转φ度后

写成矩阵形式：

根据上述推导建立下列三维直角坐标系：

如附图所示，以坐标系的三个坐标轴X，Y，Z分别作为旋转轴，则点实际上只在垂直坐标轴的平面上进行二维旋转。假设绕X轴旋转φ度，那么：

旋转前：

旋转后：

则绕X轴旋转φ的旋转矩阵为

同理可得Y轴、Z轴的旋转矩阵

2.无人机的建模

由上述旋转矩阵解出无人机完整的旋转矩阵：

设无人机所受合力为F，其中包含无人机所受重力和四个电机为其提供的与重力相反的升力F1～F4。所以根据牛二律：

令u1为高度z的控制规律

u₁＝F₁+F₂+F₃+F₄ (11)

那么

将其展开，并加入空气阻力等其他扰动：

3.位置控制器的设计

由于四旋翼无人机具有六个自由度，但是无人机只有四个驱动电机，所以四旋翼无人机的控制变量较少，四旋翼无人机具有不可避免的欠驱动特性，所以我们无法对所有的自由度进行测量控制。在本文的研究中，我们选择跟踪无人机的位置和翻滚角，凭借位置和翻滚角的稳定控制来保证另外两个角度。

假设在集群控制的主从系统中，主机位置确定，地面站向所有从机发送主机位置，并要求所有从机的位置距主机达到一个指定距离。我们假设控制目标：

首先我们设计四旋翼无人机的位置控制系统：

根据式(18)，我们定义：

那么式(18)变成：

针对X轴方向的位置系统，我们根据上述PID公式设计PD控制器。

其中k_px为PID比例部分的参数，k_dx为PID微分部分参数，x为位置误差，为位置误差的微分，下同。(由于我们设定x→0，所以误差为x-0＝x)那么：

因为控制规律为二阶连续系统，我们可以使用劳斯判据或者用赫尔维兹稳定判据，鉴于后者计算比较方便，我们采用后者的稳定型判据来确定系统参数即：已知一个二系统： a₂s²+a₁s+a₀＝0根据赫尔维兹稳定判据可得，当系统稳定时a₀＞0，a₁＞0，a₂＞0，a₁a₀＞0那么公式(22)的参数范围为：k_px＞0，k_dx+K₁/m＞0我们取k_px＝5，k_dx＝5。同理对于第二个位置系统u_2x，我们同样设计PD控制系统其中

根据赫尔维兹稳定判据可得，当系统稳定时，k_py＞0，k_dy+K₂/m＞0我们取k_py＝5，k_dy＝5。第三个位置系统是竖直方向的位置控制系统，由于无人机受到重力影响，我们设计通过前馈控制的重力补偿PD控制系统：

这里的z_e是三维坐标系统Z轴上位置分量的误差。z_e＝z-z_d其中z_d是期望值。那么

同样根据赫尔维兹稳定判据可得，当系统稳定时，k_pz＞0，我们取k_pz＝5，k_dz＝5；

4.姿态控制器的设计

为了满足上述位置控制器所要求的姿态角度θ_d和ψ_d(这里的角度是期望值)需要实际角对于期望角度进行跟踪。根据上式可得：

由于上式可以变为：

因为u_1z＝u₁cosφcosψ且所以

我们可以得到：即：

我们可以得到：

这里如果公式(25)左边的绝对值超出1，那么θ_d将不存在，在实践中我们采用将等式左边的值用MATLAB函数进行归一化，但是会造成系统的姿态角度真实值失真，所以这里我们采用一种简单的限幅算法：

我们取当X＞1时，我们取sinθ_d＝1即当X＜-1时，我们取sinθ_d＝-1即当|X|≤1时，有sinθ_d＝X那么：

此时为了使公式(25)成立，我们可以将其写为

这里ε_1xε_1y，ε_1z为不定的实数值，它的大小根据X的变化而变化，例如当|X|≤1时ε＝0。ε相当于是加在控制器上的扰动，目的是为了保证公式(26)的成立，而它对系统稳定性的影响，将由控制器自身去克服。在求解θ_d和ψ_d后我们便得到位置控制规律为：

已知无人机的姿态系统数学建模为：

定义θ_e＝θ-θ_d，设计前馈补偿的PD控制算法：

则从而根据赫尔维兹稳定判据可得，当系统稳定时，k_p4＞0，我们取k_p4＝15，k_d4＝15 取ψ_e＝ψ-ψ_d，针对第二个姿态角：

那么从而根据赫尔维兹稳定判据可得，当系统稳定时k_p5＞0，我们取k_p5＝15，k_d5＝15。

定义φ_e＝φ-φ_d，所以对第三个姿态角：

根据赫尔维兹稳定判据可得，当系统稳定时k_p6＞0，我们取k_p6＝15，k_d6＝15

5.仿真与分析

S函数

S函数是Simulink中最具魅力的核心函数模块之一，它通过程序员自主设计定义函数功能实现特定的需求，增强拓展了Simulink的功能。S函数是System Function的简称，通过一些特定的Matlab语法编程封装成模块，在Simulink中使用。

S函数内部设计了6种初始子函数，在设计S函数最开始时，要对所需要使用的相应的子函数进行初始化编程，当flag＝0时，S函数进行内部初始化工作(mdlInitializeSizes)，初始化内容包括采样时间、状态初始条件和size数组等。Size数组中程序员需要对接下来所需要用到的包括：

NumContStates(连续状态变量的个数)；

NumDiscStates(离散状态变量的个数)；

NumOutputs(S函数输出变量的个数)；

NumInputs(S函数输入变量的个数)；

DirFeedthrough(是否直接贯通)；

NumSampleTimes(采样时间的个数)进行定义。

当flag＝1时(mdlDerivatives)，可以对内部变量进行求导算；flag＝2(mdlupdate)可以对内部的变量进行更新，flag＝3(mdloutput)是计算S函数的输出；flag＝4 (mdlgetTimeNextVarHit)是计算下一个采样时间，flag＝9(mdlTerminate)是终止仿真子函数。

在对我们的模型进行仿真时，我们需要通过S函数对我们所需要的变量进行微分，这里我们利用S函数的微分函数设计使用了一种微分器来进行对变量进行微分，因为在设计时位置系统和姿态系统都是多输入多输出的系统，不仅需要在S函数中设定声明外，我们还需要用到Simulink中的MUX模块和DEMUX模块，其中MUX模块可以将多个变量的信号组成一个向量的形式输入给S函数，S函数内部分解该向量，并将相应的变量赋值给对应的中间变量便于计算。而DEMUX模块的功能与MUX模块的功能相反。

微分器的设计

使用微分器对信号进行微分是PID控制领域中的关键一环，尤其是PD控制和PID控制，因为控制器需要误差信号的微分，包括各种补偿控制都需要对变量进行求一阶导、二阶导甚至高阶导。那么微分器的计算速度便极大的影响了控制器的工作速度，从而影响系统的稳定性。

在上文设计的姿态控制系统中，我们需要对外环输出的信号ψ_d和θ_d求一次导和二次导，所以采用下述有限时间收敛的三阶微分器：

其中，v(t)为待微分信号；ε＝0.04；x₁是对待微分信号的跟踪；x₂是一阶导数的估计值； x₃是二阶导数的估计值；初始值均为0。因为该微分器的自身估值特性，可以对非连续函数进行求导，因此我们不需要要求ψ_d和θ_d是连续函数。

仿真分析

Simulink主程序和仿真结果如附图所示。

在这里我们设定从机初始位置坐标(0，0，0)，翻滚角为0度，结果如上图所示，设置位置期望值(0，0，10)，翻滚角为60度，从机的姿态和位置在5秒内达到期望值并稳定下来。可以看出双闭环PD系统对于无人机姿态位置的控制速度很快稳定性很高，几乎没有超调量和稳态误差。为了检验控制系统的抗干扰能力，我们设计干扰模块，通过Signal Builder施加干扰信号，在时间为4时，给位置变量即无人机高度施加幅值为10 的单位阶跃信号。

采用双机群控策略所涉及的双闭环系统，当主从集群无人机中的主机或者地面站给从机发送指令信号后，从机无人机的姿态和位置会迅速收敛达到稳定值，不会在群控飞行中发生碰撞。在有限的干扰情况下，位置控制系统仍然能够克服扰动并迅速达到稳定，体现了该系统的鲁棒性和可行性。但本文在设计之初时采用双闭环PID控制方案时由于误差积分过快数值过大，无法发挥积分模块的作用，但由于双闭环PD控制系统已经解决了稳态误差的问题，在仿真分析中可以看到，无论是是否存在干扰，双闭环PD系统都能够很好的克服超调量和稳态误差的问题，超调量、稳态误差和调节时间在无人机群控中都有很高的要求，本发明涉及的双闭环PD系统可以很好的达到零超调量和零稳态误差，调节时间也非常短，因而本发明的可行性较高。

Claims (1)

1.第1，

1.旋转矩阵的推导：

通过旋转矩阵建立无人机的姿态变化模型，采用[φ，θ，ψ]表示无人机的三个姿态的欧拉角度，分别代表翻滚角、俯仰角和偏航角。在二维平面中，如附图2所示在XOY平面中有一向量旋转φ度后变为

根据附图可得：旋转φ度后

写成矩阵形式：

根据上述推导建立下列三维直角坐标系：

如附图所示，以坐标系的三个坐标轴X，Y，Z分别作为旋转轴，则点实际上只在垂直坐标轴的平面上进行二维旋转。假设绕X轴旋转φ度，那么：

旋转前：

旋转后：

则绕X轴旋转φ的旋转矩阵为

同理可得Y轴、Z轴的旋转矩阵

第2，

2.无人机的建模

由上述旋转矩阵解出无人机完整的旋转矩阵：

设无人机所受合力为F，其中包含无人机所受重力和四个电机为其提供的与重力相反的升力F1～F4。所以根据牛二律：

令u1为高度z的控制规律

u₁＝F₁+F₂+F₃+F₄ (11)

那么

将其展开，并加入空气阻力等其他扰动：

第3，

3.位置控制器的设计

由于四旋翼无人机具有六个自由度，但是无人机只有四个驱动电机，所以四旋翼无人机的控制变量较少，四旋翼无人机具有不可避免的欠驱动特性，所以我们无法对所有的自由度进行测量控制。在本文的研究中，我们选择跟踪无人机的位置和翻滚角，凭借位置和翻滚角的稳定控制来保证另外两个角度。

假设在集群控制的主从系统中，主机位置确定，地面站向所有从机发送主机位置，并要求所有从机的位置距主机达到一个指定距离。我们假设控制目标：

首先我们设计四旋翼无人机的位置控制系统：

根据式(18)，我们定义：

那么式(18)变成：

针对X轴方向的位置系统，我们根据上述PID公式设计PD控制器。

其中k_px为PID比例部分的参数，k_dx为PID微分部分参数，x为位置误差，为位置误差的微分，下同。(由于我们设定x→0，所以误差为x-0＝x)那么：

因为控制规律为二阶连续系统，我们可以使用劳斯判据或者用赫尔维兹稳定判据，鉴于后者计算比较方便，我们采用后者的稳定型判据来确定系统参数即：已知一个二系统：a₂s²+a₁s+a₀＝0根据赫尔维兹稳定判据可得，当系统稳定时a₀＞0，a₁＞0，a₂＞0，a₁a₀＞0那么公式(22)的参数范围为：k_px＞0，k_dx+K₁/m＞0我们取k_px＝5，k_dx＝5。同理对于第二个位置系统u_2x，我们同样设计PD控制系统其中

根据赫尔维兹稳定判据可得，当系统稳定时，k_py＞0，k_dy+K₂/m＞0我们取k_py＝5，k_dy＝5。第三个位置系统是竖直方向的位置控制系统，由于无人机受到重力影响，我们设计通过前馈控制的重力补偿PD控制系统：

这里的z_e是三维坐标系统Z轴上位置分量的误差。z_e＝z-z_d其中z_d是期望值。那么

同样根据赫尔维兹稳定判据可得，当系统稳定时，我们取k_pz＝5，k_dz＝5；

第4，

4.姿态控制器的设计

为了满足上述位置控制器所要求的姿态角度θ_d和ψ_d(这里的角度是期望值)需要实际角对于期望角度进行跟踪。根据上式可得：

由于上式可以变为：

因为u_1z＝u₁cosφ cosψ且所以

我们可以得到：即：

我们可以得到：

这里如果公式(25)左边的绝对值超出1，那么θ_d将不存在，在实践中我们采用将等式左边的值用MATLAB函数进行归一化，但是会造成系统的姿态角度真实值失真，所以这里我们采用一种简单的限幅算法：

我们取当X＞1时，我们取sinθ_d＝1即当X＜-1时，我们取sinθ_d＝-1即当|X|≤1时，有sinθ_d＝X那么：

此时为了使公式(25)成立，我们可以将其写为

这里ε_1x，ε_1y，ε_1z为不定的实数值，它的大小根据X的变化而变化，例如当|X|≤1时ε＝0。ε相当于是加在控制器上的扰动，目的是为了保证公式(26)的成立，而它对系统稳定性的影响，将由控制器自身去克服。在求解θ_d和ψ_d后我们便得到位置控制规律为：

已知无人机的姿态系统数学建模为：

定义θ_e＝θ-θ_d，设计前馈补偿的PD控制算法：

则从而根据赫尔维兹稳定判据可得，当系统稳定时，我们取k_p4＝15，k_d4＝15

取ψ_e＝ψ-ψ_d，针对第二个姿态角：

那么从而根据赫尔维兹稳定判据可得，当系统稳定时k_p5＞0，我们取k_p5＝15，k_d5＝15。

定义φ_e＝φ-φ_d，所以对第三个姿态角：

根据赫尔维兹稳定判据可得，当系统稳定时我们取k_p6＝15，k_d6＝15。