王日俊,白 越,曾志强,段能全,党长营,杜文华,王俊元

(1.中北大学机械工程学院,太原 030051;2.中国科学院长春光学精密机械与物理研究所,长春 130033)

四旋翼无人飞行器具有结构简单、稳定性好以及起降灵活等特点,在军事和民用领域得到了极大关注与广泛应用[1-2]。搭载于飞行器上的惯性测量单元IMU(Inertial Measurement Units)在其导航信息获取与姿态运动控制中起着至关重要的作用,是保证飞行器安全可靠飞行、执行各类飞行任务的前提[3]。然而,机载环境的温度变化、机械振动等因素的存在使得IMU传感器元件极易损伤或失效,由此导致的传感器故障时有发生[4-5]。传感器一旦发生故障不仅会严重影响飞行的安全性和可靠性,导致飞行任务的失败,甚至会危及地面人员的生命安全。因此对于飞行器传感器故障检测与诊断技术的研究就成为了提高飞行器的安全性和可靠性的迫切任务。

目前大多数应用于四旋翼无人飞行器的故障诊断方法都只涉及执行器故障,而传感器故障检测和隔离FDI(Fault Detection and Isolation)的研究成果仍然有限[6]。在文献[7]提出的传感器故障全局观测器方法之后,相关研究人员逐渐将多种观测器或估计器方法应用于四旋翼无人飞行器的传感器故障检测与隔离的研究工作中,包括基于线性参数变化自适应观测器[8]、集员估计器[9],非线性状态观测器[10]和降阶非线性观测器[11]等。Freddi A 等采用基于全姿态测量的Thau观测器实现了加速度计偏差故障的检测[12]。在假设四旋翼无人飞行器的平移加速度为零时,Berbra C等提出了一种陀螺仪,加速度计和磁力计故障的检测方法[13]。此外,利用卡尔曼滤波KF(Kalman Filter)方法同时估计四旋翼飞行器状态和传感器偏差,也取得了一定的效果[14-15]。然而,对非线性系统参数估计的扩展卡尔曼滤波EKF(Extended Kalman Filter)或无迹卡尔曼滤波UKF(Unscented Kalman Filter)的稳定性分析是非常困难的,引入的额外的系统状态变量代表未知的传感器故障参数,使其难以满足有限的传感器测量的可观测条件。

本文针对四旋翼无人飞行器的加速度计和陀螺仪传感器的故障检测问题,提出一种基于非线性自适应观测器的传感器故障检测与故障偏差估计方法。该方法利用非线性故障观测器实现故障的检测与隔离,基于非线性自适应观测器同时对未知故障的大小进行估计,从而提高飞行器的容错能力,保证飞行的安全性和可靠性。

图1 四旋翼无人飞行器的结构

1 系统描述

四旋翼无人飞行器的结构如图1所示。图中,OxByBzB为机体坐标系,原点O取飞行器的质心。OxEyEzE为惯性坐标系,惯性坐标系的原点与飞行器起飞位置时机体坐标系的原点重合。Mi(i=1,2,3,4)为四组由直流电机和旋翼组成的执行机构,其中,M2、M4顺时针旋转,M1、M3逆时针旋转。Fi(i=1,2,3,4)为4个执行机构提供的升力。飞行器通过调整4个旋翼之间的转速差来完成各个方向上的姿态转动控制。

由牛顿-欧拉运动学方程得到的四旋翼无人器动力学模型可描述为[16]:

(1)

式中:m为飞行器的质量,g为重力加速度,pE为惯性坐标系下的位置,且pE∈3。vE为惯性坐标系下的速度,且vE∈3。η=[φθφ]T分别表示滚转、俯仰与偏航的欧拉角。ω=[pqr]T分别表示滚转、俯仰与偏航的角速度。Jx,Jy,Jz分别表示飞行器绕x,y,z轴的转动惯量。U表示由旋翼电机产生的总升力。Tφ,Tθ,Tφ分别表示作用于飞行器绕x,y,z轴的转动的力矩。cdvB表示作用于机体的阻力,cd为阻力系数,vB为机体坐标系下沿x,y,z方向上的速度,且vB=[uvw]T。REB为机体坐标系到惯性坐标系的转化矩阵,且,

(2)

Rη(φ,θ)表示机体坐标系下旋转速度到姿态角的角速度的转换矩阵,且

(3)

四旋翼无人飞行器通常搭载有成本低、质量轻的 MEMS传感器如3轴陀螺仪、加速度计等作为其惯性测量单元IMU。这些传感器在四旋翼无人飞行器的导航信息获取与姿态运动控制中起着至关重要的作用。通常,加速度计与陀螺仪的测量值可表示为:

ya(t)=a(t)+λa(t-ta)ba+na(t)

(4)

yω(t)=ω(t)+λω(t-tω)bω+nω(t)

(5)

式中,ya(t)为实际加速度计测量值,yω(t)为实际陀螺仪测量值,ba为加速度计测量中的故障偏差值,bω为陀螺仪测量中的故障偏差值,na(t)、nω(t)分别为加速度计和陀螺仪的测量噪声。a(t)表示无偏差故障与测量噪声时加速度计测量值,且,

(6)

式中,ta,tω分别为加速度计和陀螺仪故障的发生时刻,λa(t-ta),λω(t-tω)是关于故障时间的阶跃函数,即:

(7)

(8)

在实际的应用中,当加速度计和陀螺仪发生故障后,故障偏差值ba、bω的大小会随时间缓慢地变化,但在短时间内的偏差值的变化量很小[17]。因此,在考虑的短时间持续时间内可认为故障偏差值是恒定的。于是有如下假设。

假设1:加速度计和陀螺仪的故障偏差值ba和bω均为有界的常数,即:

(9)

(10)

图2 传感器故障检测与诊断系统结构

式中:ba=[bubvbw]T表示加速度计的故障偏差值,bω=[bpbqbr]T表示陀螺仪的故障偏差值。

假设2:在式(4)和(5)中定义的的测量噪声na(t)、nω(t)均为有界的零均值信号,且E(na(t))=0,E(nω(t))=0,E(·)为期望算子。假设惯性坐标系下四旋翼无人飞行器的位置和偏航角可测量得到。则整个系统模型的输出方程可表示为:

yp(t)=pE(t)+dp(t)

(11)

式中,dp(t)为零均值测量噪声,且E(dp(t))=0。

2 传感器故障检测与诊断系统设计

将如式(5)和(6)所示的传感器模型代入四旋翼无人飞行器动力学模型有:

(12)

从式(12)可知,加速度计测量中的偏差只出现在表示位置和速度的动力学方程中,而陀螺仪测量中的偏差只出现在表示欧拉角和角速率动力学方程中。因此,传感器测量中的偏差的影响可以被视为虚拟的执行器故障。根据上述分析,本文所设计的四旋翼无人飞行器的传感器FDD(Fault Detection and Diagnosis)系统结构如图2所示。根据FDD系统实现的功能可将结构分为故障检测与隔离FDI、故障偏差值估计FBE(Fault Bias Estimation)两部分。图中控制器的输出信号与传感器的测量值作为FDI的输入。通常情况下,两个FDI分别用来实现加速度计和陀螺仪测量过程中故障的检测与隔离,一旦其中某一故障被检测到并隔离,便会激活与之对应的FEB完成对故障偏差值的估计,从而增强飞行器控制系统的容错能力,保证飞行器的稳定性与安全性。

2.1 传感器的故障观测器设计

2.1.1 加速度计故障观测器

由式(12)可知,四旋翼无人飞行器的质心运动方程可表示为:

(13)

设计如下加速度计故障观测器:

(14)

(15)

(16)

(17)

2.1.2 陀螺仪故障观测器

根据式(12),设计如下陀螺仪故障观测器:

(18)

(19)

当t

(20)

(21)

2.2 传感器故障偏差的非线性自适应观测器设计

2.2.1 加速度计故障偏差观测器

根据式(12),用以估计加速度计故障偏差值的非线性自适应观测器可表示为:

(22)

基于Lyapunov方法设计的参数自适应律为:

(23)

定理1假设加速度计传感器发生故障在ta时,在Ta时刻检测到故障,且Ta>ta>0。那么,对于在任意t>Ta时,如果存在常数α1≥α0>0和T0>0,有:

(24)

证根据式(13)与式(22)可得,状态变量估计误差的表达式为:

(25)

(26)

(27)

(28)

对式(27)进行期望值的计算,结合E[na(t)]=0和E[dp(t)]=0,可得:

(29)

同样,对式(28)进行期望值的计算,可得:

(30)

定理得证。

2.2.2 陀螺仪故障偏差观测器

根据式(12),基于Lyapunov方法设计用以估计陀螺仪故障偏差的自适应观测器为:

(31)

为了保证参数的收敛性,Rη(φ,θ)需满足如下条件:

(32)

证如式(31)所示的自适应观测器的状态与参数的估计误差可表示为:

(33)

由假设2有,nω有界,则扰动项Rη(φ,θ)nω也有界,显然式(33)中的信号均有界。

对式(33)进行期望值的计算,结合E[nω(t)]=0,可得:

(34)

(35)

式中,03×3表示3×3零矩阵。根据文献[20]的定理可知,[ξ1ξ2]T=[0 0]T是指数收敛于零的。

定理得证。

3 实验研究

搭建的室内实验平台系统结构框图如图3所示。整个系统由课题组研制的四旋翼无人飞行器、Vicon运动捕捉系统以及地面站三部分组成。在飞行过程中,四旋翼无人飞行器的姿态信息和位置信息通过Vicon运动捕捉系统获取,采样频率1 Hz,通过TCP/IP协议实现与地面站的通信。地面站通过XTend远距离射频模块连接四旋翼无人飞行器,实现数据快速可靠的传输与参数的在线调整。

图3 室内实验平台系统结构框图

为了验证本文设计的故障检测与诊断系统,在四旋翼无人飞行器在空中时,分别向加速度计和陀螺仪测量中引入人为设定的偏差,其中引入的加速度计故障偏差为ba=[0.15 0.2 0.75]T(m/s-2),引入的陀螺仪故障偏差bω=[5 -7 -10]T((°)/s)。加速度计和陀螺仪故障的引入时序如图4所示。其中,加速度计故障的引入时间为t=20 s,陀螺仪故障的引入时间为t=50 s。此外,为了验证本方法在多个故障情况下的有效性,在时间t=100 s时同时引入加速度计故障和陀螺仪故障,如图4所示。

图4 故障设置时序图

3.1 加速度计故障实验

在t=20 s时,在加速度计故障偏差ba=[0.15 0.20 0.75]T(m/s-2)引入后,通过故障观测器对故障进行检测的实验结果如图5所示。其中,图5(a)为加速度计故障观测器的观测结果,可以看出加速度计故障观测器能够从20 s开始快速实现对故障的检测。图5(b)为陀螺仪故障观测器的观测结果,此时陀螺仪故障观测器并未检测到故障的发生,与式(13)所描述的一致,即这两种传感器故障是解耦的。

图5 故障检测

在加速度计故障检测到之后,便会激活与之对应的加速度计偏差估计器实现对故障偏差值的估计。对故障偏差的估计验证实验结果如图6所示。从图中可以看出,加速度计在20 s处发生故障,对故障偏差的估计值大约在15 s之后接近给定值,实现了对加速度计故障偏差的准确估计与跟踪。

图6 加速度计故障偏差估计

3.2 陀螺仪故障实验

在t=50 s时,陀螺仪故障bω=[5 -7 -10]T((°)/s)引入后,通过故障观测器对故障检测的实验结果如图7所示。其中,图7(a)为加速度计故障观测器的观测结果,可以看出加速度计故障观测器并未检测到故障的发生。图5(b)为陀螺仪故障观测器的观测结果,此时陀螺仪故障观测器能够从50 s开始快速实现对陀螺仪故障的检测,且两者解耦。

图7 陀螺仪故障检测

在陀螺仪故障检测到之后,便会激活与之对应的陀螺仪偏差估计器实现对故障偏差值的估计。对故障偏差的估计验证实验结果如图8所示。从图中可以看出,在50 s处发生故障后,非线性故障偏差估计器均能够在故障产生5 s后完成对故障偏差的估计,且接近设定值。本文设计的陀螺仪故障偏差观测器能够完成对陀螺仪故障偏差的准确估计与跟踪。

图8 陀螺仪故障偏差估计

图9 基于CUSUM的加速度计和陀螺仪同时故障残差分析

3.3 加速度计与陀螺仪同时故障实验

在t=100 s时,加速度计和陀螺仪同时发生故障,即同时引入加速度计故障ba=[0.15 0.2 0.75]T(m/s-2)和陀螺仪故障bω=[5 -7 -10]T((°)/s)。为了增强对故障残差均值微小偏移的检测,利用基于残差均值的双侧累积和控制图CUSUM(Cumulative Sum)对残差进行统计分析,结果如图9所示。其中图9(a)为加速度计的故障残差CUSUM统计结果,图9(b)陀螺仪的故障残差CUSUM统计结果,检测阈值用来检测传感器故障。从图中可以看出,故障发生后,与两个故障观测器相对应的残差统计量均在很短的时间内超过各自的检测阈值。由此可知,加速度计和陀螺仪均发生了故障,本文设计的故障观测器能够有效地检测到同时发生的两种传感器故障。

图11 同时故障时陀螺仪故障偏差估计

在同时检测到两种故障后,对应的偏差观测器对故障偏差值的估计结果分别如图10和图11所示。其中,图10为同时故障时对加速度计故障偏差的估计结果,图11为同时故障时对陀螺仪故障偏差的估计结果。可以看出,本文设计的两种偏差观测器均能够实现对故障偏差的快速估计与有效跟踪。

图10 同时故障时加速度计故障偏差估计

4 结论

构造快速、准确的传感器故障诊断环节是提高四旋翼无人飞行器系统的容错能力,保证其在飞行作业过程中安全飞行的重要基础。本文针对四旋翼无人飞行器传感器故障的诊断问题,设计了基于自适应观测器的四旋翼无人飞行器传感器故障诊断方法。对所提出的诊断方法进行了理论分析,并通过数值仿真和实际飞行验证了所提出方法的有效性。实验结果表明所设计的故障观测器可快速地检测到故障的发生位置,并实现故障隔离。在传感器发生故障后,自适应故障偏差观测器能够准确估计传感器故障偏差值,有效地提高了飞行器的可靠性和安全性。