技术领域
[0001] 本
发明属于计算机技术与无人机结合的技术领域，特别是涉及一种应用在无人机上的双机群控策略。
背景技术
[0002] 无人机是人们通过无线通讯设备和自动控制装置通讯控制不载人的飞机。英文简称 UAV，最早在20世纪20年代出现，因其具备的独特功能性和世界科学技术的发展，无人机被广泛应用。
[0003] 无人机也被称作无人驾驶
机器人，二战期间，无人机在应用于军事打击时取得了很重要的作用后，大量的研究人员和爱好者投入到无人机的研究中。在军事领域中，无人机的应用于军事打击、目标
跟踪、侦查等，在民用领域中无人机被大量爱好者用于航拍，近年来国内外也开始应用无人机进行灭火、植保等。在无人机不断发展不断普及的情况下，美国、中国等国家相继制定诸多有关无人机的政策，例如无人机的实名登记政策等。今天在公园上空嗡嗡作响，对飞机造成干扰的四轴无人机其实早在1962年就
申请了
专利。 Piasecki Aircraft Corporation的工程师Edward G.Vanderlip最先设计了让
直升机仪器在动
力失效的情况下仍能继续工作的手段。当今社会越来越多的科技公司投入到无人机的研究中，无人机的集群控制更是今年来无人机领域的研究热点。国外的学者和专家对于无人机的研究以已有100多年之久，尤其是美国和以色列。其中内华达大学的专家，开发一架可以帮助患者行走的无人机。国内的大疆公司、小米公司等多家科技公司都已上市众多无人机产品。清华大学、北京航空航天大学等诸多高校也正对无人机做许多重点技术的研究。在集群控制的不断发展下，人们也渐渐将无人机表演当做一种环保且新奇的娱乐方式。由于无人机自身的特性，许多高校的学生都会选择用无人机作为研究对象，这也大大推动了无人机的发展。但无人机并不仅仅局限于现有，现今世界对无人机的研究仅仅只是一个开始，随着人类数量的增长，科技的发展，地球上可用的移动空间越来越小，无人机凭借其体型小且灵活的优势，可以在未来的生活中担任城市侦查、物流等任务。且就像无人
汽车的发展一样，在未来的某一天，我们也终会乘坐无人驾驶飞机去旅行。
[0004] 由于无人机的不断发展和普及，越来越多的无人机爱好者不断开发拓展无人机的功能，无人机集群控制是近年来新兴的科技产业，无人机集群控制是通过群控
算法在各个无人机相互通讯的情况下，由两个或多个无人机针对任务要求，保持某种
姿态队形进行飞行，通过算法控制多架无人机协同飞行的技术。基于集群控制的无人机机群有着诸多的优势，包括：飞行耗能低，任务执行率高，高效率等。由于无人机飞行算法的不断完善和小型民用无人机的发展，越来越多的人开始研究无人机用于舞
台灯光表演。
发明内容
[0005] 本发明所要解决的技术问题是，设计一种应用在无人机上的双机群控策略，采用了双闭环PD算法设计主从双机无人机集群控制系统，用matlab进行了仿真，由于双闭环 PD系统的良好的鲁棒性，克服了无人机的欠驱动特性，在仿真过程中，发现对于
位置和姿态的控制有着良好的
稳定性和动态特性。并将此算法应用于实际，有着良好的收益。
[0006] 本发明提供的无人机上的双机群控策略包括：
[0007] 1.旋转矩阵的推导：
[0008] 通过旋转矩阵建立无人机的姿态变化模型，采用[φ，θ，ψ]表示无人机的三个姿态的欧拉
角度，分别代表翻滚角、
俯仰角和
偏航角。在二维平面中，如
附图2所示在XOY平面中有一向量 旋转φ度后变为
[0009] 根据附图可得： 旋转φ度后
[0010]
[0011] 写成矩阵形式：
[0012]
[0013] 根据上述推导建立下列三维直角
坐标系：
[0014] 如附图所示，以坐标系的三个坐标轴X，Y，Z分别作为旋
转轴，则点实际上只在垂直坐标轴的平面上进行二维旋转。假设 绕X轴旋转φ度，那么：
[0015]
[0016] 旋转前：
[0017]
[0018] 旋转后：
[0019]
[0020] 则绕X轴旋转φ的旋转矩阵为
[0021]
[0022] 同理可得Y轴、Z轴的旋转矩阵
[0023]
[0024]
[0025] 2.无人机的建模
[0026] 由上述旋转矩阵解出无人机完整的旋转矩阵：
[0027]
[0028] 设无人机所受合力为F，其中包含无人机所受重力和四个
电机为其提供的与重力相反的升力F1～F4。所以根据
牛二律：
[0029]
[0030] 令u1为高度z的控制规律
[0031] u1＝F1+F2+F3+F4 (11)
[0032] 那么
[0033]
[0034] 将其展开，并加入
空气阻力等其他扰动：
[0035]
[0037] 由于四旋翼无人机具有六个
自由度，但是无人机只有四个
驱动电机，所以四旋翼无人机的控制变量较少，四旋翼无人机具有不可避免的欠驱动特性，所以我们无法对所有的自由度进行测量控制。在本文的研究中，我们选择跟踪无人机的位置和翻滚角，凭借位置和翻滚角的稳定控制来保证另外两个角度。
[0038] 假设在集群控制的主从系统中，主机位置确定，地面站向所有从机发送主机位置，并要求所有从机的位置距主机达到一个
指定距离。我们假设控制目标：
[0039]
[0040] 首先我们设计四旋翼无人机的位置控制系统：
[0041] 根据式(18)，我们定义：
[0042]
[0043] 那么式(18)变成：
[0044]
[0045] 针对X轴方向的位置系统，我们根据上述PID公式设计PD控制器。
[0046]
[0047] 其中kpx为PID比例部分的参数，kdx为PID微分部分参数，x为位置误差，为位置误差的微分，下同。(由于我们设定x→0，所以误差为x-0＝x)那么：
[0048]
[0049] 因为控制规律为二阶连续系统，我们可以使用劳斯判据或者用赫尔维兹稳定判据，鉴于后者计算比较方便，我们采用后者的稳定型判据来确定系统参数即：已知一个二系统： a2s2+a1s+a0＝0根据赫尔维兹稳定判据可得，当系统稳定时a0＞0，a1＞0，a2＞0，a1a0＞0 那么公式(22)的参数范围为：kpx＞0，kdx+K1/m＞0我们取kpx＝5，kdx＝5。同理对于第二个位置系统u2x，我们同样设计PD控制系统 其中
[0050]
[0051] 根据赫尔维兹稳定判据可得，当系统稳定时，kpy＞0，kdy+K2/m＞0我们取kpy＝5，kdy＝5。第三个位置系统是竖直方向的位置控制系统，由于无人机受到重力影响，我们设计通过前馈控制的重力补偿PD控制系统：
[0052]
[0053] 这里的ze是三维坐标系统Z轴上位置分量的误差。ze＝z-zd其中zd是期望值。那么[0054]
[0055] 同样根据赫尔维兹稳定判据可得，当系统稳定时，kpz＞0， 我们取kpz＝5，kdz＝5；
[0056] 4.姿态控制器的设计
[0057] 为了满足上述位置控制器所要求的姿态角度θd和ψd(这里的角度是期望值)需要实际角对于期望角度进行跟踪。根据上式可得：
[0058]
[0059] 由于 上式可以变为：
[0060] 因为u1z＝u1cosφcosψ且 所以
[0061]
[0062] 我们可以得到：
[0063] 我们可以得到：
[0064]
[0065] 这里如果公式(25)左边的绝对值超出1，那么θd将不存在，在实践中我们采用将等式左边的值用MATLAB函数进行归一化，但是会造成系统的姿态角度真实值失真，所以这里我们采用一种简单的
限幅算法：
[0066] 我们取 当X＞1时，我们取sinθd＝1即 当X＜-1 时，我们取sinθd＝-1即 当|X|≤1时，有sinθd＝X那么：
[0067]
[0068] 此时为了使公式(25)成立，我们可以将其写为
[0069]
[0070] 这里ε1x，ε1y，ε1z为不定的实数值，它的大小根据X的变化而变化，例如当|X|≤1时ε＝0。ε相当于是加在控制器上的扰动，目的是为了保证公式(26)的成立，而它对系统稳定性的影响，将由控制器自身去克服。在求解θd和ψd后我们便得到位置控制规律为：
[0071]
[0072] 已知无人机的姿态系统数学建模为：
[0073]
[0074] 定义θe＝θ-θd，设计前馈补偿的PD控制算法：
[0075]
[0076] 则 从而 根据赫尔维兹稳定判据可得，当系统稳定时，kp4＞0， 我们取kp4＝15，kd4＝15 取ψe＝ψ-ψd，针对第二个姿态角：
[0077]
[0078] 那么 从而 根据赫尔维兹稳定判据可得，当系统稳定时kp5＞0， 我们取kp5＝15，kd5＝15。
[0079] 定义φe＝φ-φd，所以对第三个姿态角：
[0080]
[0081] 根据赫尔维兹稳定判据可得，当系统稳定时kp6＞0，我们取kp6＝15，kd6＝15
附图说明
[0082] 图1二维平面中向量 的变化示意图
[0083] 图2三维坐标系中 的变化
[0085] 图4仿真的主程序
[0086] 图5仿真的结果1
[0087] 图6仿真的结果2
[0088] 图7仿真的结果3
[0090] 图9仿真的结果4
[0091] 图10仿真的结果5
[0092] 图11仿真的结果6
具体实施方式
[0093] 实例1
[0094] 1.旋转矩阵的推导：
[0095] 通过旋转矩阵建立无人机的姿态变化模型，采用[φ，θ，ψ]表示无人机的三个姿态的欧拉角度，分别代表翻滚角、俯仰角和偏航角。在二维平面中，如附图2所示所示在XOY 平面中有一向量 旋转φ度后变为
[0096] 根据附图可得： 旋转φ度后
[0097]
[0098] 写成矩阵形式：
[0099]
[0100] 根据上述推导建立下列三维直角坐标系：
[0101] 如附图所示，以坐标系的三个坐标轴X，Y，Z分别作为
旋转轴，则点实际上只在垂直坐标轴的平面上进行二维旋转。假设 绕X轴旋转φ度，那么：
[0102]
[0103] 旋转前：
[0104]
[0105] 旋转后：
[0106]
[0107] 则绕X轴旋转φ的旋转矩阵为
[0108]
[0109] 同理可得Y轴、Z轴的旋转矩阵
[0110]
[0111]
[0112] 2.无人机的建模
[0113] 由上述旋转矩阵解出无人机完整的旋转矩阵：
[0114]
[0115] 设无人机所受合力为F，其中包含无人机所受重力和四个电机为其提供的与重力相反的升力F1～F4。所以根据牛二律：
[0116]
[0117] 令u1为高度z的控制规律
[0118] u1＝F1+F2+F3+F4 (11)
[0119] 那么
[0120]
[0121] 将其展开，并加入空气阻力等其他扰动：
[0122]
[0123] 3.位置控制器的设计
[0124] 由于四旋翼无人机具有六个自由度，但是无人机只有四个驱动电机，所以四旋翼无人机的控制变量较少，四旋翼无人机具有不可避免的欠驱动特性，所以我们无法对所有的自由度进行测量控制。在本文的研究中，我们选择跟踪无人机的位置和翻滚角，凭借位置和翻滚角的稳定控制来保证另外两个角度。
[0125] 假设在集群控制的主从系统中，主机位置确定，地面站向所有从机发送主机位置，并要求所有从机的位置距主机达到一个指定距离。我们假设控制目标：
[0126]
[0127] 首先我们设计四旋翼无人机的位置控制系统：
[0128] 根据式(18)，我们定义：
[0129]
[0130] 那么式(18)变成：
[0131]
[0132] 针对X轴方向的位置系统，我们根据上述PID公式设计PD控制器。
[0133]
[0134] 其中kpx为PID比例部分的参数，kdx为PID微分部分参数，x为位置误差，为位置误差的微分，下同。(由于我们设定x→0，所以误差为x-0＝x)那么：
[0135]
[0136] 因为控制规律为二阶连续系统，我们可以使用劳斯判据或者用赫尔维兹稳定判据，鉴于后者计算比较方便，我们采用后者的稳定型判据来确定系统参数即：已知一个二系统： a2s2+a1s+a0＝0根据赫尔维兹稳定判据可得，当系统稳定时a0＞0，a1＞0，a2＞0，a1a0＞0 那么公式(22)的参数范围为：kpx＞0，kdx+K1/m＞0我们取kpx＝5，kdx＝5。同理对于第二个位置系统u2x，我们同样设计PD控制系统 其中
[0137]
[0138] 根据赫尔维兹稳定判据可得，当系统稳定时，kpy＞0，kdy+K2/m＞0我们取kpy＝5，kdy＝5。第三个位置系统是竖直方向的位置控制系统，由于无人机受到重力影响，我们设计通过前馈控制的重力补偿PD控制系统：
[0139]
[0140] 这里的ze是三维坐标系统Z轴上位置分量的误差。ze＝z-zd其中zd是期望值。那么[0141]
[0142] 同样根据赫尔维兹稳定判据可得，当系统稳定时，kpz＞0， 我们取kpz＝5，kdz＝5；
[0143] 4.姿态控制器的设计
[0144] 为了满足上述位置控制器所要求的姿态角度θd和ψd(这里的角度是期望值)需要实际角对于期望角度进行跟踪。根据上式可得：
[0145]
[0146] 由于 上式可以变为：
[0147] 因为u1z＝u1cosφcosψ且 所以
[0148]
[0149] 我们可以得到： 即：
[0150]
[0151] 我们可以得到：
[0152]
[0153] 这里如果公式(25)左边的绝对值超出1，那么θd将不存在，在实践中我们采用将等式左边的值用MATLAB函数进行归一化，但是会造成系统的姿态角度真实值失真，所以这里我们采用一种简单的限幅算法：
[0154] 我们取 当X＞1时，我们取sinθd＝1即 当X＜-1 时，我们取sinθd＝-1即 当|X|≤1时，有sinθd＝X那么：
[0155]
[0156] 此时为了使公式(25)成立，我们可以将其写为
[0157]
[0158] 这里ε1xε1y，ε1z为不定的实数值，它的大小根据X的变化而变化，例如当|X|≤1时ε＝0。ε相当于是加在控制器上的扰动，目的是为了保证公式(26)的成立，而它对系统稳定性的影响，将由控制器自身去克服。在求解θd和ψd后我们便得到位置控制规律为：
[0159]
[0160] 已知无人机的姿态系统数学建模为：
[0161]
[0162] 定义θe＝θ-θd，设计前馈补偿的PD控制算法：
[0163]
[0164] 则 从而 根据赫尔维兹稳定判据可得，当系统稳定时，kp4＞0， 我们取kp4＝15，kd4＝15 取ψe＝ψ-ψd，针对第二个姿态角：
[0165]
[0166] 那么 从而 根据赫尔维兹稳定判据可得，当系统稳定时kp5＞0， 我们取kp5＝15，kd5＝15。
[0167] 定义φe＝φ-φd，所以对第三个姿态角：
[0168]
[0169] 根据赫尔维兹稳定判据可得，当系统稳定时kp6＞0，我们取kp6＝15，kd6＝15
[0170] 5.仿真与分析
[0171] S函数
[0172] S函数是Simulink中最具魅力的核心函数模
块之一，它通过程序员自主设计定义函数功能实现特定的需求，增强拓展了Simulink的功能。S函数是System Function的简称，通过一些特定的Matlab语法编程封装成模块，在Simulink中使用。
[0173] S函数内部设计了6种初始子函数，在设计S函数最开始时，要对所需要使用的相应的子函数进行初始化编程，当flag＝0时，S函数进行内部初始化工作 (mdlInitializeSizes)，初始化内容包括
采样时间、状态初始条件和size数组等。Size数组中程序员需要对接下来所需要用到的包括：
[0174] NumContStates(连续状态变量的个数)；
[0175] NumDiscStates(离散状态变量的个数)；
[0176] NumOutputs(S函数输出变量的个数)；
[0177] NumInputs(S函数输入变量的个数)；
[0178] DirFeedthrough(是否直接贯通)；
[0179] NumSampleTimes(采样时间的个数)进行定义。
[0180] 当flag＝1时(mdlDerivatives)，可以对内部变量进行求导算；flag＝2(mdlupdate)可以对内部的变量进行更新，flag＝3(mdloutput)是计算S函数的输出；flag＝4 (mdlgetTimeNextVarHit)是计算下一个采样时间，flag＝9(mdlTerminate)是终止仿真子函数。
[0181] 在对我们的模型进行仿真时，我们需要通过S函数对我们所需要的变量进行微分，这里我们利用S函数的微分函数设计使用了一种微分器来进行对变量进行微分，因为在设计时位置系统和姿态系统都是多输入多输出的系统，不仅需要在S函数中设定
声明外，我们还需要用到Simulink中的MUX模块和DEMUX模块，其中MUX模块可以将多个变量的信号组成一个向量的形式输入给S函数，S函数内部分解该向量，并将相应的变量赋值给对应的中间变量便于计算。而DEMUX模块的功能与MUX模块的功能相反。
[0182] 微分器的设计
[0183] 使用微分器对信号进行微分是PID控制领域中的关键一环，尤其是PD控制和PID 控制，因为控制器需要误差信号的微分，包括各种补偿控制都需要对变量进行求一阶导、二阶导甚至高阶导。那么微分器的计算速度便极大的影响了控制器的工作速度，从而影响系统的稳定性。
[0184] 在上文设计的姿态控制系统中，我们需要对外环输出的信号ψd和θd求一次导和二次导，所以采用下述有限时间收敛的三阶微分器：
[0185]
[0186] 其中，v(t)为待微分信号；ε＝0.04；x1是对待微分信号的跟踪；x2是一阶导数的估计值； x3是二阶导数的估计值；初始值均为0。因为该微分器的自身估值特性，可以对非连续函数进行求导，因此我们不需要要求ψd和θd是连续函数。
[0187] 仿真分析
[0188] Simulink主程序和仿真结果如附图所示。
[0189] 在这里我们设定从机初始位置坐标(0，0，0)，翻滚角为0度，结果如上图所示，设置位置期望值(0，0，10)，翻滚角为60度，从机的姿态和位置在5秒内达到期望值并稳定下来。可以看出双闭环PD系统对于无人机姿态位置的控制速度很快稳定性很高，几乎没有超调量和稳态误差。为了检验控制系统的抗干扰能力，我们设计干扰模块，通过Signal Builder施加
干扰信号，在时间为4时，给位置变量即无人机高度施加幅值为10 的单位阶跃信号。
[0190] 采用双机群控策略所涉及的双
闭环系统，当主从集群无人机中的主机或者地面站给从机发送指令信号后，从机无人机的姿态和位置会迅速收敛达到稳定值，不会在群控飞行中发生碰撞。在有限的干扰情况下，位置控制系统仍然能够克服扰动并迅速达到稳定，体现了该系统的鲁棒性和可行性。但本文在设计之初时采用双闭环PID控制方案时由于误差积分过快数值过大，无法发挥积分模块的作用，但由于双闭环PD控制系统已经解决了稳态误差的问题，在仿真分析中可以看到，无论是是否存在干扰，双闭环PD系统都能够很好的克服超调量和稳态误差的问题，超调量、稳态误差和调节时间在无人机群控中都有很高的要求，本发明涉及的双闭环PD系统可以很好的达到零超调量和零稳态误差，调节时间也非常短，因而本发明的可行性较高。
