一种四旋翼无人机的终端协同控制方法
【专利摘要】一种四旋翼无人机的终端协同控制方法，针对四旋翼无人机系统，基于牛顿欧拉公式，建立四旋翼无人机的动态模型；解耦四旋翼无人机的位置与姿态耦合关系，以便于分别控制；在系统状态空间中分别构造位置终端流形和姿态终端流形，以加快系统误差收敛速度；根据相应的流形动态特性，分别设计位置终端协同控制器和姿态终端协同控制器，以消除控制输入抖振现象；最后，通过解耦结果串联两控制器，构造内外双环实现四旋翼无人机的飞行控制。本发明提供一种四旋翼无人机的终端协同控制方法，消除控制输入抖振现象，提高系统响应速度，实现四旋翼无人机的无抖振快速控制。
【专利说明】
一种四旋翼无人机的终端协同控制方法
技术领域
[0001] 本发明涉及一种四旋翼无人机的终端协同控制方法，适用于四旋翼无人机无抖振 快速飞行控制。
【背景技术】
[0002] 四旋翼无人机作为旋翼式飞行器的一种，结构简单、成本低廉，可操作性和机动性 强，通过控制四个旋翼转速实现的飞行控制能够方便地完成起飞与降落等动作，在执行近 地面侦察、监视等任务时极具优势。因此，拥有独特飞行控制方式的四旋翼无人机具有广泛 的应用前景，成为国内外研究热点之一。然而，四旋翼无人机是一类具有欠驱动、强耦合以 及静不稳定特点的非线性系统。对于这类复杂系统，实现快速稳定的控制存在一定难度。因 此，实现对四旋翼无人机有效的飞行控制已经成为一个亟待解决的问题。
[0003] 针对四旋翼无人机的飞行控制问题，许多控制方法被提出，如PID控制方法、滑模 控制方法、自抗扰控制方法等。其中，滑模控制方法在解决系统不确定性和外部扰动方面被 认为是一个有效的鲁棒控制方法。它具有算法简单、响应速度快、对外界噪声干扰和参数摄 动鲁棒性强等优点。然而，滑模控制本质上的不连续开关特性会引起控制输入抖振问题，易 损坏电源和电机，成为其在实际应用中的障碍。
【发明内容】
[0004] 为了克服现有的四旋翼无人机飞行控制方法中存在的控制输入抖振和响应速度 较慢的问题，本发明提供一种四旋翼无人机的终端协同控制方法，保证系统稳定可靠，同时 加快系统响应速度，消除控制输入抖振现象，实现四旋翼无人机的无抖振快速控制。
[0005] 为了解决上述技术问题提出的技术方案如下：
[0006] -种四旋翼无人机的终端协同控制方法，包括以下步骤：
[0007] 步骤1，建立四旋翼无人机的动态模型，过程如下：
[0008] 1.1在忽略空气阻力和陀螺效应的基础上，对四旋翼无人机进行以下假设:无人机 是刚性的;无人机的结构是完全对称的;无人机的重心与机体坐标系原点重合;并定义机体 坐标系到惯性坐标系的转移矩阵R为：
[0010] 其中，Φ、θ、φ分别为无人机的偏航角、俯仰角、翻滚角；
[0011] 1.2基于牛顿公式，从受力角度分析无人机，平动过程中有：
[0013]其中，m表示无人机的质量，g表示重力常数，UF表示无人机四个旋翼产生的合升 力，X、y、z表示无人机在惯性坐标系下各轴的位置，(?〕表示二阶导数；
[0014] 将式(1)代入式(2)中得到：
[0016] 1.3基于欧拉公式，从受力角度分析无人机，转动过程中有：
[0018] 其中，τχ、τγ、τζ分别代表机体坐标系下各轴力矩分量，Ix、I y、Iz分别代表机体坐标 系下各轴转动惯量分量，P、q、r分别代表机体坐标系下各轴姿态角速度分量，於、6、f分别 代表机体坐标系下各轴姿态角加速度分量；
[0019] 将式(4)表示为：
[0021]考虑到四旋翼无人机处于低速飞行或者悬停状态下，认为廣* g、??γ、.多《ρ、. 湯《?将式(5)改写为：
[0023] 1.4因此系统的动态模型重写为：
[0027]步骤2,计算系统位置跟踪误差，设计位置流形，选择动态特性，过程如下：
[0028] 2.1定义系统位置跟踪误差为：
[0029] ew=w-wd (9)
[0030] 其中，w=[x;y;z]，wd=[xd;yd;zd]分别表示实际和期望的位置轨迹；
[0031 ]那么式(9)的一阶微分和二阶微分分别表不为：
[0032] ew = W - WA (10):
[0033] ew ： = w- % (11)
[0034] 2.2宙^份詈流形为，
[0036]其中，s = [sx;sy;sz]，<11>〇，01>〇，卩 1和91是正奇数，满足卩1〈91〈2卩1;
[0037] 那么式（12)的一阶微分表示为：
[0039] 2.3选择位置流形期望的动态特性方程为：
[0040] T^v+sw=. 0 (.14)
[0041] 其中，1\>0,决定了系统状态变量收敛到流形sw=0的速度；
[0042] 步骤3,基于四旋翼无人机的动态模型，根据动态特性方程，设计位置协同控制器， 过程如下：
[0043] 3.1将式（11)、式（12)和式（13)代入式（14)中得到：
[0045] 3.2考虑式(7)和式(15)，位置协同控制器被设计为：
[0047] 其中，u=[Ux;Uy;Uz];
[0048] 3.3设计李雅普诺夫函数：
[0052]因此，系统位置跟踪误差能收敛至零，表明系统是稳定的；
[0053]步骤4,位置与姿态关系解耦，过程过下：
[0054] 4.1对式(8)解耦计算，得到：
[0056]其中，加，<i>d，9d为期望姿态角，加由人为给定；
[0057]步骤5，计算系统姿态跟踪误差，设计姿态流形，选择动态特性，过程如下：
[0058] 5.1定义系统姿态跟踪误差为：
[0059] θω= Ω -Ω a (20)
[0060] 其中，Ω = [ φ ;θ;φ]，Qd=[ (J)d;0d;i])d]分别表示实际和期望的姿态轨迹；
[0061 ] 那么式(20)的一阶微分和二阶微分表不为：
[0062] esi = Ω - ?(1 (21)
[0063] ea = Ω.-α(, (22)
[0064] 5.2定义姿态流形为：
[0066] 其中，SQ = [Sx;Sy;Sz]，<12>〇，02>〇，？2和92是正奇数，满足?2〈92〈2卩2;
[0067] 那么式(23)的一阶微分表示为：
[0069] 5.3选择姿态流形期望的动态特性方程为：
[0070] + Sq - 〇 (2:5)
[0071] 其中，T2>0,决定了系统状态变量收敛到流形％ = 0的速度；
[0072] 步骤6,基于四旋翼无人机的动态模型，根据动态特性方程，设计姿态协同控制器， 过程如下：
[0073] 6.1将式(22)、式(23)和式(24)代入式(25)中得到：
[0075] 6.2考虑式(7)和式(26)，姿态协同控制器被设计为：
[0077] 其中，T = [Tx;Ty;T：z]，b = [bi;b2;b3]，f(n) =
[0078] 6.3设计李雅普诺夫函数：
[0080]对式(28)进行微分，并代入式(27)得到：
[0082] 因此，系统姿态跟踪误差能收敛至零，表明系统是稳定的。
[0083] 本发明基于耦合非线性的系统模型，分别设计位置终端协同控制器和姿态终端协 同控制器，并通过解耦计算串联两控制器，保证系统稳定可靠，同时加快系统响应速度，消 除控制输入抖振现象，实现四旋翼无人机的无抖振快速控制。
[0084] 本发明的技术构思为:基于牛顿欧拉公式，建立四旋翼无人机的系统模型。解耦四 旋翼无人机的位置与姿态耦合关系，以便于分别控制。在系统状态空间中分别构造位置终 端流形和姿态终端流形。该终端流形中含有非线性项，能加快系统误差收敛速度。根据相应 的流形动态特性，分别设计位置终端协同控制器和姿态终端协同控制器。协同控制器由于 工作在固定频率，能消除控制输入抖振现象。最后，通过解耦结果串联两控制器，构造内外 双环实现四旋翼无人机的飞行控制。本发明提供一种消除控制输入抖振现象，提高系统响 应速度的控制方法，实现四旋翼无人机的无抖振快速控制。
[0085] 本发明的有益效果为:消除抖振现象，加快动态响应，实现快速稳定控制。
【附图说明】
[0086] 图1为本发明的四旋翼无人机位置跟踪效果示意图，其中，（a)表示X轴位置跟踪效 果，（b)表示y轴位置跟踪效果，（c)表示z轴位置跟踪效果。
[0087] 图2为本发明的四旋翼无人机姿态角收敛效果示意图，其中，（a)表示翻滚角收敛 效果，（b)表示俯仰角收敛效果，（c)表示偏航角收敛效果。
[0088] 图3为本发明的四旋翼无人机位置控制器输入示意图，其中，（a)表示位置控制器 在X轴方向的输入，（b)表示位置控制器在y轴方向的输入，（c)表示位置控制器在z轴方向的 输入。
[0089] 图4为方法S1中四旋翼无人机的姿态控制器输入示意图，其中，（a)表示该姿态角 控制器在X轴方向的输入，（b)表示该姿态角控制器在y轴方向的输入，（c)表示该姿态角控 制器在z轴方向的输入。
[0090] 图5为方法S2中四旋翼无人机的姿态控制器输入示意图，其中，（a)表示该姿态角 控制器在X轴方向的输入，（b)表示该姿态角控制器在y轴方向的输入，（c)表示该姿态角控 制器在z轴方向的输入。
[0091] 图6为本发明的四旋翼无人机姿态控制器输入示意图，其中，（a)表示该姿态角控 制器在X轴方向的输入，（b)表示该姿态角控制器在y轴方向的输入，（c)表示该姿态角控制 器在z轴方向的输入。
[0092]图7为本发明的控制流程示意图。
【具体实施方式】
[0093] 下面结合附图对本发明做进一步说明。
[0094] 参照图1-图7,一种四旋翼无人机的终端协同控制方法，包括以下步骤：
[0095] 步骤1，建立四旋翼无人机的动态模型，过程如下：
[0096] 1.1在忽略空气阻力和陀螺效应的基础上，对四旋翼无人机进行以下假设:无人机 是刚性的;无人机的结构是完全对称的;无人机的重心与机体坐标系原点重合;并定义机体 坐标系到惯性坐标系的转移矩阵R为：
[0098]其中，Φ、θ、φ分别为无人机的偏航角、俯仰角、翻滚角；
[0099] 1.2基于牛顿公式，从受力角度分析无人机，平动过程中有：
[0101]其中，m表示无人机的质量，g表示重力常数，UF表示无人机四个旋翼产生的合升 力，X、y、Z表示无人机在惯性坐标系下各轴的位置，(S)表示二阶导数；
[0102] 将式(1)代入式(2)中得到：
[0104] 1.3基于欧拉公式，从受力角度分析无人机，转动过程中有：
[0106]其中，τχ、τγ、τζ分别代表机体坐标系下各轴力矩分量，I x、Iy、Iz分别代表机体坐标 系下各轴转动惯量分量，P、q、r分别代表机体坐标系下各轴姿态角速度分量，4、. #分别 代表机体坐标系下各轴姿态角加速度分量；
[0107] 将式(4)表示为：
[0109]但考虑到四旋翼无人机一般处于低速飞行或者悬停状态下，姿态角变化较小，此 时认为多 wjj、.否_?.4、将式(5)改写为：
[0111] 1.4因此系统的动态模型重写为：
[0115]步骤2，计算系统位置跟踪误差，设计位置流形，选择动态特性，过程如下：
[0116] 2.1定义系统位置跟踪误差为：
[0117] ew=w-wd (9)
[0118] 其中，w=[x;y;z]，wd=[xd;yd;zd]分别表示实际和期望的位置轨迹；
[0119] 那么式(9)的一阶微分和二阶微分分别表不为：
[0120] ?w = W- Wd (1Θ)
[0121 ] = # - (11)
[0122] 2.2定义位置流形为：
[0124] 其中，s = [sx;sy;sz]，<11>〇，01>〇，卩 1和91是正奇数，满足卩1〈91〈2卩1;
[0125] 那么式（12)的一阶微分表示为：
[0127] 2.3选择位置流形期望的动态特性方程为：
[0128] T^+Sw^O (14)
[0129] 其中，1\>0,决定了系统状态变量收敛到流形sw=0的速度；
[0130] 步骤3,基于四旋翼无人机的动态模型，根据动态特性方程，设计位置协同控制器， 过程如下：
[0131] 3.1将式(11)、式(12)和式(13)代入式(14)中得到：
[0133] 3.2考虑式(7)和式(15)，位置协同控制器被设计为：
[0135]其中，u=[Ux;Uy;Uz];
[0136] 3.3设计李雅普诺夫函数：
[0138]对式(17)进行微分，并代入式(16)得到：
[0140] 因此，系统位置跟踪误差能收敛至零，表明系统是稳定的；
[0141] 步骤4,位置与姿态关系解耦，过程过下：
[0142] 4 · 1对式(8)解耦计算，得到：
[0144]其中，加，<i>d，9d为期望姿态角，加由人为给定；
[0145] 步骤5，计算系统姿态跟踪误差，设计姿态流形，选择动态特性，过程如下：[0146] 5.1定义系统姿态跟踪误差为：[0147] θω= Ω -Ω a (20)[0148] 其中，Ω = [ φ ;θ;φ]，Qd=[ (J)d;0d;i])d]分别表示实际和期望的姿态轨迹；[0149] 那么式(20)的一阶微分和二阶微分表不为：[0150] ea = Ω - (21:)[0151] e.a = Ω - ??(1 (22):[0152] 5.2定义姿态流形为：
[0154] 其中，SQ = [Sx;Sy;Sz]，<12>〇，02>〇，？2和92是正奇数，满足?2〈92〈2卩2;[0155] 那么式(23)的一阶微分表示为：
[0157] 5.3选择姿态流形期望的动态特性方程为：[0158] + sn = 0 (25)[0159] 其中，Τ2>0,决定了系统状态变量收敛到流形％ = 0的速度；[0160] 步骤6,基于四旋翼无人机的动态模型，根据动态特性方程，设计姿态协同控制器， 过程如下：[0161] 6.1将式(22)、式(23)和式(24)代入式(25)中得到：
[0163] 6.2考虑式(7)和式(26)，姿态协同控制器被设计为：
[0165] 其中，T = [Tx;Ty;T：z]，b = [bi;b2;b3]，f(i!) =[0166] 6.3设计李雅普诺夫函数：
[0168]对式(28)进行微分，并代入式(27)得到：
[0170]因此，系统姿态跟踪误差能收敛至零，表明系统是稳定的。[0171]为了验证所提方法的有效性，本发明给出了以下三种方法进行对比：[0172] S1:快速终端滑模控制方法；[0173] S2:普通协同控制方法；[0174] S3:本发明的终端协同控制方法。[0175]为了更有效的进行对比，系统所有初始状态和模型参数都是一致的，即：X = y = z =0, φ = θ= φ = 〇，ηι = 0· 625kg，Ix = 2 · 3 X 10-3kg · m2，Iy = 2.4X 10-3kg · m2，Iz = 2.6X10- 31^.11124=1〇111.8-2;流形与控制器参数:€[ 1 = 2，01 = 1，口1 = 5,91 = 7，1'1 = 0.2，€[2 = 2，02 = 1， P2 = 5，q2 = 7，T2 = 0.2;参考轨迹：xd = yd = zd = 2，itd = 0.5。
[0176] 由图1可以看出，方法S3较方法S1和方法S2,更早跟上期望信号，大约提前0.5秒， 可见方法S3响应速度更快，飞行器的快速跟踪性能更佳；由图2可以看出，方法S3较方法S1 和方法S2,其中翻滚角和俯仰角收敛至零的时间更短，且稳态时轨迹平滑，偏航角到达期望 值时间基本一致；由图3可以看出，由于位置期望值固定，各方法中位置控制器的输入都几 乎不存在抖振情况；由图4可以看出，由于姿态期望值通过实时解耦计算得到，而滑模控制 中存在开关切换函数，方法S1中姿态控制器的输入存在明显的抖振现象；由图5可以看出， 方法S2中不存在抖振现象，体现了普通协同控制方法的去抖振效果；由图6可以看出，方法 S3中只在0.8秒处存在一较小幅值的冲击值，其余各处输入平滑，仍具有较好的去抖振效 果。综上所述，方法S3在加快响应速度的基础上，仍具有普通协同控制的去抖振效果，能够 实现四旋翼无人机的快速无抖振控制。
[0177] 以上阐述的是本发明给出的一个实施例表现出的优良优化效果，显然本发明不只 是限于上述实施例，在不偏离本发明基本精神及不超出本发明实质内容所涉及范围的前提 下对其可作种种变形加以实施。
【主权项】
1. 一种四旋翼无人机的终端协同控制方法，其特征在于:所述终端协同控制方法包括 W下步骤： 步骤1，建立四旋翼无人机的动态模型，过程如下： 1.1在忽略空气阻力和巧螺效应的基础上，对四旋翼无人机进行W下假设:无人机是刚 性的;无人机的结构是完全对称的;无人机的重屯、与机体坐标系原点重合;并定义机体坐标 系到惯性坐标系的转移矩阵R为：其中，Φ、Θ、Φ分别为无人机的偏航角、俯仰角、翻滚角； 1.2基于牛顿公式，从受力角度分析无人机，平动过程中有：(2) 其中，m表示无人机的质量，g表示重力常数，化表示无人机四个旋翼产生的合升力，x、y、 Z表示无人机在惯性坐标系下各轴的位置，巧)表示二阶导数； 将式(1)代入式(2)中得到：其中，Tx、Ty、Tz分别代表机体坐标系下各轴力矩分量，IxJy、Iz分别代表机体坐标系下 各轴转动惯量分量，P、q、r分别代表机体坐标系下各轴姿态角速度分量，护、r分别代表 机体坐标系下各轴姿态角加速度分量. 将式(4)表示为：诚 考虑到四旋翼无人机处于低速飞行或者悬停状态下，认为Φ 3 P ,房《 g、φκρ、吕^弓、嗔W户，将式巧)改写为；步骤2，计算系统位置跟踪误差，设计位置流形，选择动态特性，过程如下： 2.1定义系统位置跟踪误差为： ew=w-wd (9) 其中，w=[x;y;z]，wd=[xd;yd;zd]分别表示实际和期望的位置轨迹； 那么式(9)的一阶微分和二阶微分分别表示为：其中，s = [Sx;Sy;Sz]，日1〉〇，01〉〇，口1和91是正奇数，满足口1<91<化1; 那么式(12)的一阶微分表示为：其中，Τι〉0，决定了系统状态变量收敛到流形Sw= 0的速度； 步骤3，基于四旋翼无人机的动态模型，根据动态特性方程，设计位置协同控制器，过程 如下： 3.1将式(11)、式(12)和式(13)代入式(14)中得到：3.2考虑式(7)和式(15)，位置协同控制器被设计为：因此，系统位置跟踪误差能收敛至零，表明系统是稳定的； 步骤4，位置与姿态关系解禪，过程过下： 4.1对式(8)解禪计算，得到：其中，也1，为期望姿态角，也1由人为给定； 步骤5，计算系统姿态跟踪误差，设计姿态流形，选择动态特性，过程如下： 5.1定义系统姿态跟踪误差为： ΘΩ= Ω-Qd (20) 其中，Ω = [ Φ ;θ;φ]，Qd=[ Φ(?;θ<ι;Φ(?]分别表示实际和期望的姿态轨迹； 那么式(20)的一阶微分和二阶微分表示为：其中，SQ = [Sx;Sy;Sz]，日2〉〇，02〉〇，口2和92是正奇数，满足口2<92<化2; 那么式(23)的一阶微分表示为：(1:4) 5.3选择姿态流形期望的动态特性方程为：牌） 其中，Τ2〉0,决定了系统状态变量收敛到流形3Ω = 0的速度； 步骤6，基于四旋翼无人机的动态模型，根据动态特性方程，设计姿态协同控制器，过程 如下： 6.1将式(22)、式(23)和式(24)代入式(25)中得到：因此，系统姿态跟踪误差能收敛至零，表明系统是稳定的。
【文档编号】G05D1/08GK106094855SQ201610607798
【公开日】2016年11月9日
【申请日】2016年7月27日 公开号201610607798.5, CN 106094855 A, CN 106094855A, CN 201610607798, CN-A-106094855, CN106094855 A, CN106094855A, CN201610607798, CN201610607798.5
【发明人】陈强, 王音强, 卢敏, 庄华亮, 孙明轩, 何熊熊
【申请人】浙江工业大学