四旋翼飞行器容错控制方法

1.一种四旋翼飞行器容错控制方法，其特征在于包括如下步骤：
步骤1：根据飞行器的数学模型，建立飞行器存在系统故障情况下的姿态模型；
其中，所述步骤1中的姿态模型为：
式中：Θ为飞行器的姿态角；Ω为飞行器绕质心运动的角速率；β(t-tf)是一个单位阶跃函数，表示系统在tf时刻出现故障；ρ1、ρ2分别表示飞行器姿态角回路和角速率回路的系统故障大小；W为姿态角与绕质心运动的角速率之间的转换关系；J为飞行器的惯性矩阵；Ga为飞行器由电机转动而产生的陀螺力矩；M为飞行器的控制分配矩阵； 为飞行器的控制输入；
步骤2：根据步骤1中所述的姿态模型，推出飞行器系统故障估计的自适应律；
步骤3：根据步骤1中所述的姿态模型，建立飞行器执行器失效故障的传递函数；
步骤4：根据步骤3中所述的传递函数，得出飞行器执行器失效故障估计的自适应律；
步骤5：根据步骤1中所述的姿态模型和步骤3中所述的传递函数，建立飞行器的复合故障姿态模型；
步骤6：根据步骤5中所述的复合故障姿态模型，设计出飞行器存在系统故障以及飞行器执行器失效故障下的容错控制器，从而实现飞行器的容错控制。
2.根据权利要求1所述的四旋翼飞行器容错控制方法，其特征在于：所述步骤2中的系统故障估计的自适应律为：
式 中： 是系统故障估计向量；
分 别 为Θ、Ω 的 观 测 值； 为 滤 波 器 的 输 出，滤 波 器 为
其 中κ1,κ2＞ 0 是 (3×3) 的
矩阵； 表示在tf时刻ρ1和ρ2的故障值；γ1、γ2为对角参数矩阵；
其中 为系统故障估计误差。
3.根据权利要求1所述的四旋翼飞行器容错控制方法，其特征在于：所述步骤3中的传递函数为：
式中：s为传递函数符号； 为故障执行器的输出量； 为故障执行器的输入量；ki为故障执行器的输出增益；α＝1/Tm，Tm为惯性时间常数；i＝1,2,3,4。
4.根据权利要求1所述的四旋翼飞行器容错控制方法，其特征在于：所述步骤4中的执行器失效故障估计的自适应律为：
式中：μ为设计的常数；ei为故障执行器与正常执行器传递函数的广义误差； 为执行器发生故障时的输出转速；t为时间。
5.根据权利要求1所述的四旋翼飞行器容错控制方法，其特征在于：所述步骤5中的复合故障姿态模型为：
式中：K＝diag{k1,k2,k3,k4}，其中k1、k2、k3、k4为飞行器四个故障执行器的输出增益；
为执行器的控制输入。
6.根据权利要求1所述的四旋翼飞行器容错控制方法，其特征在于：所述步骤6中的容错控制器包括：
1)姿态角回路控制器：
d d
式中：c1为第一正定对角阵；ZΘ＝Θ-Θ ，Θ为飞行器的期望姿态角指令；
Φ1为 的上界， 为姿态角回路的系统故障估计误差， 为姿态角
回路的系统故障估计向量；
2)角速率回路控制器：
d
式中：c2为第二正定对角阵；ZΩ＝Ω-Ω ； 其中
为飞行器四个执行器失效故障的估计值； Φ2为 的上界， 为角速
率回路的系统故障估计误差， 为角速率回路的系统故障估计向量；uvsc为非线性滑模项。

四旋翼飞行器容错控制方法\n技术领域\n[0001] 本发明涉及一种四旋翼飞行器的容错控制方法，属于航空飞行器控制领域。\n背景技术\n[0002] 具有垂直起降、稳定悬停和自主巡航能力的微小型四旋翼飞行器，在军事和民事领域具有广阔的应用前景，可广泛应用于航拍摄影、电力巡检、环境监测、森林防火、灾情巡查、防恐救生、军事侦察、战场评估等领域。该飞行器普遍采用电能驱动，具有结构简单、飞行稳定、易于操控、低噪声、无污染、携带方便、安全危害性小等特点，非常适合于执行中短距离的飞行任务。\n[0003] 四旋翼飞行器的体量一般定位于微小型，机载资源相对紧凑，同时，执行机构(电机和旋翼组件)的数量较多，在执行任务过程中难免会发生系统故障，其姿态控制系统的故障一般分为执行器、传感器和系统这三种类型的故障。因此，要改善飞行器的安全性和可靠性，在设计稳定的姿态控制系统时，还必须考虑故障诊断与辨识以及容错控制。\n[0004] 然而，国内外对四旋翼飞行器的容错控制研究并不多。文献(Farid Sharifi,Mostafa Mirzaei,Brandon W.Gordon,Youmin Zhang.Fault tolerant control of a quadrotor UAV using sliding mode control.2010Conference on Control and Fault Tolerant Systems,2010:239-244.)、(Xiaobing Zhang,Youmin Zhang,Chun-Yi Su,Ying Feng.Fault tolerant control for quadrotor via backstepping approach.48th AIAA Aerospace Sciences Meeting Including the New Horizons Forum and Aerospace Exposition,2010.)、(C.Berbra,S.Lesecq,J.J.Martinez.A multi-observer switching strategy for fault-tolerant control of a quadrotor helicopter.16th Mediterranean Conference on Control and Automation,2008:1094-1099.)、(杨新哲，姜斌，陈复扬，张柯.基于多观测器的一类过驱动系统主动容错控制设计)分别采用滑模控制、backstepping、多观测器切换策略对四旋翼飞行器进行容错控制，但上述文献均未涉及到执行器动态。设计执行器的故障诊断和辨识非常困难，但如果在设计一个飞行器故障诊断和辨识过程中不考虑执行器动态模型，那么在后续的半实物仿真和飞行测试中可能都会遇到阻碍。\n发明内容\n[0005] 本发明的目的在于：提出一种四旋翼飞行器的容错控制方法，以实现飞行器在发生系统故障或执行器失效故障时仍能保持姿态稳定。\n[0006] 该方法包括如下步骤：\n[0007] 步骤1：根据飞行器的数学模型，建立飞行器存在系统故障情况下的姿态模型；\n[0008] 步骤2：根据步骤1中所述的姿态模型，推出飞行器系统故障估计的自适应律；\n[0009] 步骤3：根据步骤1中所述的姿态模型，建立飞行器执行器失效故障的传递函数；\n[0010] 步骤4：根据步骤3中所述的传递函数，得出飞行器执行器失效故障估计的自适应律；\n[0011] 步骤5：根据步骤1中所述的姿态模型和步骤3中所述的传递函数，建立飞行器的复合故障姿态模型；\n[0012] 步骤6：根据步骤5中所述的复合故障姿态模型，设计出飞行器存在系统故障以及飞行器执行器失效故障下的容错控制器，从而实现飞行器的容错控制。\n[0013] 技术效果：\n[0014] 1、本方法对四旋翼飞行器具有很好的容错控制能力，可大幅降低飞行器坠机事件发生的概率，大大提高了飞行器的飞行可靠性和安全性。\n[0015] 2、本方法中建立了飞行器系统故障时的姿态模型，使得飞行控制研究更具有针对性，且更切合物理实际。\n[0016] 3、本方法中充分考虑了执行器的动态模型，为后续的半实物仿真和飞行测试带来了便利。\n[0017] 4、本方法中的系统故障估计自适应律独立于控制器的设计，且该自适应律只由估计误差驱动，而非直接与状态跟踪或者预测误差有关。这种估计自适应律较传统的自适应律具有更快的速度直到实现参数收敛。\n[0018] 5、本方法中提出了基于执行器输入输出模型的故障估计自适应律，该自适应律很容易运用于执行器动态模型上，且算法结构简单。\n附图说明\n[0019] 图1为本发明方法的容错控制实现原理图。\n[0020] 图2为类MIT模型参考自适应的执行器故障估计结构原理图。\n[0021] 图3为实施例情况1的姿态角跟踪曲线图。\n[0022] 图4为实施例情况1的滚转、俯仰和偏航角速率响应曲线图。\n[0023] 图5为实施例情况1的系统故障估计曲线图。\n[0024] 图6为实施例情况2的姿态角跟踪曲线和无容错控制下的姿态角跟踪曲线图。\n[0025] 图7为实施例情况2的滚转、俯仰、偏航角速率响应曲线和无容错控制下的滚转、俯仰、偏航角速率响应曲线图。\n[0026] 图8为实施例情况2的执行器故障估计和实际故障曲线图。\n[0027] 图9为实施例情况3的姿态角跟踪曲线和无容错控制下的姿态角跟踪曲线图。\n[0028] 图10为实施例情况3的滚转、俯仰、偏航角速率响应曲线和无容错控制下的滚转、俯仰、偏航角速率响应曲线图。\n[0029] 图11为实施例情况3的系统故障估计、执行器故障估计和实际故障曲线图。\n具体实施方式\n[0030] 本发明四旋翼飞行器容错控制方法的容错控制实现原理如图1所示，该方法主要包括如下步骤：\n[0031] 步骤1：根据四旋翼飞行器的数学模型，建立飞行器存在系统故障情况下的姿态模型；\n[0032] 步骤2：根据步骤1中所述的姿态模型，推出飞行器系统故障估计的自适应律；\n[0033] 步骤3：根据步骤1中所述的姿态模型，建立飞行器执行器(电机)失效故障的传递函数；\n[0034] 步骤4：根据步骤3中所述的传递函数，得出飞行器执行器失效故障估计的自适应律；\n[0035] 步骤5：根据步骤1中所述的姿态模型和步骤3中所述的传递函数，建立飞行器带有系统故障和执行器失效故障的复合故障姿态模型；\n[0036] 步骤6：根据步骤5中所述的复合故障姿态模型，设计出飞行器存在系统故障以及飞行器执行器失效故障下的容错控制器，从而实现飞行器的容错控制。\n[0037] 下面对每个步骤作进一步详细说明：\n[0038] 步骤1中：飞行器存在系统故障情况下的姿态模型\n[0039] 本发明中的四旋翼飞行器存在系统故障情况下的姿态模型是在(Z.Zuo.Trajectory tracking control design with command-filter compensation for a quadrotor.IET Control Theory and Application,2010,4(11):2343-2355.)的基础上提出的，该姿态模型的具体建模过程可参见此文献，该姿态模型为：\n[0040] \n[0041] \n[0042] 式(1)、(2)中：Θ＝(φ,θ,ψ)T为飞行器的姿态角，φ为滚转角，θ为俯仰角，T\nψ为偏航角；Ω＝(p,q,r)为飞行器绕质心运动的角速率，p为滚转角速率，q为俯仰角速率，r为偏航角速率；β(t-tf)是一个单位阶跃函数，表示系统在tf时刻出现系统故障；ρ1、ρ2分别表示飞行器姿态角回路和角速率回路的系统故障大小。\n[0043] W为姿态角与绕质心运动的角速率之间的转换关系，其表达式如下：\n[0044] \n[0045] J为飞行器的惯性矩阵，其表达式如下：\n[0046] \n[0047] 式(4)中：Jx、Jy、Jz分别为机体x轴、y轴、z轴的转动惯量。\n[0048] Ga为飞行器由电机转动而产生的陀螺力矩，其表达式为：\n[0049] \nT\n[0050] 式(5)中：Jr为电机的转动惯量；ze＝(0,0,1) ；ωi为第i个电机的转速。\n[0051] M为飞行器的控制分配矩阵，其表达式为：\n[0052] \n[0053] 式(6)中：b、d分别为升力和阻力系数；l为旋翼到飞行器质心的距离。\n[0054] 为飞行器的控制输入，其表达式为： 其中 i＝\n1,2,3,4为飞行器四个执行器(电机)的转速平方。\n[0055] 步骤2中：飞行器的系统故障估计自适应律\n[0056] 四旋翼飞行器的状态Θ和Ω都是可测的，根据飞行器存在系统故障情况下的姿态模型，定义状态观测器如下：\n[0057] \n[0058] \n[0059] 式(7)中： 是系统故障估计向量；κ1,κ2＞0是(3×3)的矩阵；\n和 是预测误差， 分别为式(7)所示状态观测器的输出。\n[0060] 为下式所示滤波器的输出，滤波器为：\n[0061] \n[0062] \n[0063] 式(8)中：1∈R3×1的向量。\n[0064] 定义 和 为系统故障估计误差，由式(1)、(2)和(7)可得：\n[0065] \n[0066] \n[0067] 定义：\n[0068] \n[0069] \n[0070] 结合式(8)、(9)，则η1、η2可表示为：\n[0071] \n[0072] \n[0073] 故，系统故障估计自适应律为：\n[0074] \n[0075] \n[0076] 式(12)中：γ1、γ2为设计的对角参数矩阵； 表示在tf时刻ρ1和ρ2的故障值。\n[0077] 步骤3中：执行器失效故障的传递函数\n[0078] 直流电机的模型可以近似为惯性环节，并描述为：\n[0079] \n[0080] 式(13)中：α＝1/Tm，Tm为惯性时间常数；s为传递函数的符号；ωm(s)为直流电机的输出转速；u(s)为直流电机的输入量。\n[0081] 四旋翼飞行器的实时输入量为每个直流电机转速的平方，所以四旋翼飞行器每个执行器的实时动态模型可表示为以下的传递函数形式：\n[0082] \n[0083] 式(14)中：i＝1,2,3,4。\n[0084] 当飞行器的执行器(电机)无失效故障，式(13)和(14)的输出增益fi、ki均为\n1；当发生失效故障后，fi、ki分别降到[εi,1)和 区间上，其中εi＜1， εi为大于等于0且小于1的常数。假设fi、ki为分段的固定百分比，即 则 对时\n间的导数可写成\n[0085] 因此，发生故障后的直流电机模型和四旋翼执行器的实时传递函数可描述为：\n[0086] \n[0087] \n2\n[0088] 式(15)、(16)中：i＝1,2,3,4；ki＝fi，ki为故障执行器的输出增益； (s)为故障执行器的输出量； 为故障执行器的输入量。\n[0089] 步骤4中：执行器失效故障估计的自适应律\n[0090] 本发明提出一种基于执行器输入输出模型的故障估计自适应方法，其结构类似于MIT模型参考自适应控制，该故障估计器的结构原理如图2所示。\n[0091] 具体实现步骤如下：\n[0092] 定义故障执行器和正常执行器传递函数的广义误差为：\n[0093] \n[0094] 选取性能指标泛函数为：\n[0095] \n[0096] 当可调增益 (执行器失效故障估计值)等于ki(执行器失效故障实际值)时，* *\n则可使得性能指标J达到最小值。采用梯度法，首先求出J 对 的梯度：\n[0097] \n[0098] 根据梯度法可知，值应沿梯度下降的方向移动，在一定的步距下， 的变化量将取值为：\n[0099] \n[0100] 式(20)中λ＞0。\n[0101] 调整后的 为：\n[0102] \n[0103] 式(21)中： 为可调增益 的初始值，\n[0104] 为了获得调整 的自适应规律，式(21)的两边对时间t求导得：\n[0105] \n[0106] 由式(22)可见，为了获得调整 的自适应规律 必须计算 由图2可以看出，此类故障估计器的 的传递函数为：\n[0107] \n[0108] 将式(23)变形为：\n[0109] \n[0110] 将频域方程(24)进行Laplace反变换为时域方程：\n[0111] \n[0112] 式(25)中p为微分算子。\n[0113] 将式(25)的两边对 求导得：\n[0114] \n[0115] 而故障执行器的输入与输出有下列关系：\n[0116] \n[0117] 由式(26)和(27)可知， 和 成比例关系。为了抗干扰起见，实际系统中常避免使用微分信号 而采用故障执行器的输出，因为两者之间仅差一个比例常数，所以由式(22)得：\n[0118] \n[0119] 式(28)中：μ为设计的常数； 为执行器发生故障时的输出转速；t为时间。\n[0120] 式(28)就是执行器的失效故障估计自适应规律。\n[0121] 步骤5中：飞行器的复合故障姿态模型\n[0122] 由于四旋翼飞行器执行器(电机)的动态非常快，故式(13)中α＞＞1。将式\n2\n(16)写成微分方程并且方程两边同除以α可得：\n[0123] \n[0124] 因为α＞＞1，可得1/α2≌0，2/α≌0，则可获得低阶执行器故障动态模型为：\n[0125] \n[0126] 因此，飞行器带有系统故障和执行器失效故障的复合故障姿态模型为：\n[0127] \n[0128] 式(31)中：K＝diag{k1,k2,k3,k4}是以k1、k2、k3、k4为对角线元素的对角矩阵，k1、k2、k3、k4为飞行器四个故障执行器的输出增益(失效故障实际值)；\n为执行器的控制输入。\n[0129] 步骤6中：飞行器姿态系统的容错控制器\n[0130] 根据系统故障估计和执行器失效故障估计设计飞行器的容错控制器，其包括：\n[0131] 1)姿态角回路控制器：\n[0132] \nd d\n[0133] 式(32)中：c1为设计的第一正定对角阵；ZΘ＝Θ-Θ 为姿态角跟踪误差，Θ 为飞行器的期望姿态角指令；r1是为消除估计误差的鲁棒性， Φ1为 的上界，可表示为 为姿态角回路的系统故障估计误差， 为姿态角回路的系统故\n障估计向量。\n[0134] 2)角速率回路容错控制器：\n[0135] \nd\n[0136] 式(33)中：c2为设计的第二正定对角阵；ZΩ＝Ω-Ω 为角速率跟踪误差；\n是以 为对角线元素的对角矩阵， 为飞\n行器四个执行器失效故障的估计值；r2是为消除估计误差的鲁棒性， Φ2为 的上界，可表示为 为角速率回路系统故障估计误差， 为角速率回路\n系统的故障估计向量；uvsc为非线性滑模项。\n[0137] 下面介绍本发明的实施例。\n[0138] 以某小型四旋翼飞行器为对象，在Matlab/Simulink环境下对其进行仿真验证，以验证本发明的四旋翼飞行器容错控制方法的有效性。四旋翼飞行器数学模型的具体参数如下：m＝0.468Kg，l＝0.225m，Jx＝4.856×10-3Kg.m2，Jy＝4.856×10-3Kg.m2，Jz＝\n8.801×10-3Kg.m2，Jr＝3.357×10-5Kg.m2，b＝2.98×10-5NS2/rad2，d＝1.14×10-6NS2/rad2。\n[0139] 情况1：假设飞行器只发生系统故障，并假设系统故障为如下形式：\n[0140] \n[0141] 系统故障估计算法的参数选择为：γ1＝γ2＝diag([600,600,600])，κ1＝κ2＝diag([2,2,2])；容错控制器的参数选取为：c1＝diag{0.4,0.4,0.4}，c2＝diag{1,1,1}，其仿真结果如图3～图5所示，表示系统相应的状态(姿态角和角速率)和系统故障的估计。由仿真结果可以看出，利用在线系统故障估计、补偿以及容错控制器，当系统故障发生后，容错控制系统的状态能在短时间之内很好地跟踪设定值。可见本发明对于系统故障的影响具有很好的容错能力。\n[0142] 情况2：假设飞行器只发生执行器(电机)失效故障。\n[0143] 假定发生的执行器故障为：在t＝1s时，前面电机M1丧失效力50％，而且在t＝\n8s时，左边电机M4丧失效力60％，即四旋翼飞行器的实时执行器分别在t＝1s和t＝8s丧失效力25％和36％。\n[0144] 执行器故障估计器自适应律设计为： 容错控\n制器的参数选取为：c1＝diag{0.4,0.4,0.4}，c2＝diag{1,1,1}。当执行器在t＝1s和t＝8s发生上述失效故障时，仿真结果如图6～图8所示，表示系统相应的状态(姿态角和角速率)和执行器失效故障值的估计。由仿真结果可以看出，没有容错控制的系统，当执行器故障发生后，飞行器的姿态角会发生较大波动，而不能很好地跟踪设定值。采用本发明的容错控制器，在发生执行器失效故障后，容错控制系统的状态能在短时间之内很好地跟踪设定值。\n[0145] 情况3：系统故障和执行器故障同时存在，假设飞行器在飞行过程中先后发生第一种系统故障和第二种执行器故障，系统故障和执行器故障估计算法的参数选择同情况1和情况2。当系统在t＝5s时发生系统故障，在t＝1s、t＝8s发生如情况2所述的执行器故障，仿真结果如图9～图11所示，表示系统的相应状态(姿态角和角速率)以及系统故障和执行器失效故障值的估计。由仿真结果可以看出，利用本发明的容错控制器，在发生系统故障和执行器失效故障后，容错控制系统的状态仍能在短时间之内很好地跟踪设定值，而没有进行容错控制的系统，当执行器发生故障后，系统的状态会发生较大波动，而不能很好地跟踪设定值。\n[0146] 实验表明，本发明不仅对单类型故障有较好的容错能力，对于系统故障和执行器故障甚至多类型的故障，仍然具有很好的容错控制能力。\n[0147] 在本说明书中未作详细描述的内容属本领域技术人员的公知技术。