基于两级卡尔曼滤波器的三旋翼无人机执行器异常行为诊断

马文来, 王少波, 李瑞先, 郝 伟

(1.南京航空航天大学民航学院,南京 211000; 2.山东理工大学交通与车辆工程学院,山东 淄博 255000;3.滨州学院飞行学院,山东 滨州 256000)

0 引言

多旋翼无人机因机动灵活、可垂直起降、可定点悬停等特点得到广泛应用[1]。多旋翼无人机主要通过改变电机转速产生相应的力矩差值,实现无人机的滚转、悬停、俯仰以及偏航等动作。三旋翼无人机作为多旋翼无人机的一种,完成上述运动需要在改变电机转速的基础上,通过尾部舵机的倾转来实现[2-3]。三旋翼无人机电机、舵机以及螺旋桨等执行器的高速运转加大了其发生异常行为的风险,进而使得无人机飞行事故频繁发生,给周围的人和环境带来很大的安全隐患。同时,当三旋翼无人机执行器发生异常行为时,系统控制输入变量发生改变,使得无人机欠驱动特性更为显著,动力学特性也增加了更多的不确定性,对于无人机的稳定控制提出了严峻的挑战[4]。

当前针对三旋翼无人机执行器异常行为的研究相对较少,主要研究方向包括故障动力学建模、容错控制等。文献[5-6]针对三旋翼无人机尾部舵机异常设计了基于滑模控制算法的鲁棒控制器,并进行了实验验证;针对三旋翼无人机同时存在未知扰动和执行器异常的情况,文献[7]基于浸入-不变集方法提出一种自适应控制算法实现了对执行器异常行为的补偿;文献[8]设计了滑模观测器对执行器异常行为进行估计;针对三旋翼无人机其他方向的研究,文献[9]采用鲁棒滑模控制算法实现了对无人机姿态的延时估计;文献[10]针对三旋翼无人机的高度控制,设计了神经网络自适应控制方法并通过数值仿真进行了验证。

综上所述,目前针对三旋翼无人机执行器异常行为的研究大多采用容错控制等方式通过设计控制器对执行器异常行为进行抑制或补偿,未对其进行有效估计,使得对异常行为的抑制或补偿缺少针对性。

卡尔曼滤波(KF)是一种利用系统状态方程,通过观测系统输入输出数据,对系统状态进行最优估计的常用算法[11]。然而,应用卡尔曼滤波器只能实现对三旋翼无人机执行器异常行为的检测,无法达到故障诊断的目的。当所有数学模型和随机模型的先验精度均满足假设条件、观测向量及状态预测向量均服从正态分布时,标准卡尔曼滤波算法的解具有无偏性和方差最小性[12]。合理、准确地建立动态载体的状态运动模型是动态导航和定位过程中极重要的环节之一,而实际上由于受到诸多条件的限制,特别是随机因素的影响,在建立系统的状态运动模型时,系统状态方程中总是存在着常量或者随机的偏差量,这种偏差的存在必然会给动态导航结果带来偏差,甚至使卡尔曼滤波发散[13]。因此,本文在卡尔曼滤波的基础上对其进行改进,提出两级卡尔曼滤波(Two-Stage Kalman Filtering,TSKF)算法对三旋翼无人机执行器异常行为进行诊断。该算法在建立模型时将随机因素考虑在内,而且可以对系统状态和执行器异常行为进行估计,两级卡尔曼滤波算法是通过建立耦合方程将系统状态子滤波器以及异常行为子滤波器联系起来的,这样既可以对异常行为进行检测,又可以对异常行为进行隔离、识别和诊断,不仅减少了计算量,将部分执行器发生异常行为后对系统状态的影响程度也表现出来,而且有效弥补了卡尔曼滤波器的不足[14-15]。

1 三旋翼无人机异常动力学建模

1.1 三旋翼无人机动力学模型分析

首先建立三旋翼无人机的机体坐标系B和地球固连坐标系I,如图1所示。

图1 三旋翼无人机坐标系示意图Fig.1 Coordinate system of a tri-rotor UAV

根据欧拉定理,地球固连坐标系I绕固定点旋转后可以得到机体坐标系B,这一旋转的角度即为欧拉角。机体坐标系B绕地球固连坐标系I的xI轴旋转得到滚转角φ,绕yI旋转得到俯仰角θ,绕zI轴旋转得到偏航角ψ。根据右手定则,定义顺时针为正。参考文献[16],根据牛顿-欧拉法可以得到机体坐标系下三旋翼无人机的动力学模型为

(1)

式中:m∈R,表示三旋翼无人机的质量;V(t)=[Vx(t)Vy(t)Vz(t)]T∈R3,表示三旋翼无人机旋转的线速度;ω(t)=[ω1(t)ω2(t)ω3(t)]T∈R3,表示三旋翼无人机旋转时的角速度;J=diag(J1,J2,J3)∈R3×3,表示三旋翼无人机的转动惯量;T1(t)=[F1(t)F2(t)F3(t)]T∈R3,表示在平动方向上输入的合外力向量,F1(t)、F2(t)和F3(t)分别表示xI轴、yI轴和zI轴的合外力;τ(t)=[τθ(t)τφ(t)τψ(t)]T∈R3,表示各转动方向输入的力矩向量,τθ(t)、τφ(t)和τψ(t)分别表示俯仰、滚转、偏航通道的输入力矩。

由机体坐标系B到地球固连坐标系I的旋转矩阵R可以表示为

(2)

角速度变换矩阵Ψ可以表示为

(3)

将机体坐标系B中的动力学模型投影到地球固连坐标系I后,得到三旋翼无人机动力学模型为

(4)

当三旋翼无人机处于悬停状态时,总升力T1近似等于无人机重力,即T1=mg,且俯仰角、滚转角和偏航角均在0°附近变化,波动范围较小,因此,可对式(4)进行简化,得到

(5)

对式(5)进行离散化处理,得到离散状态空间方程为

xk+1=Akxk+U1(k)+w1k

(6)

yk+1=Ckxk+1+vk+1

(7)

1.2 执行器异常行为建模

三旋翼无人机执行器发生异常行为后系统的控制失效异常可以建模为

γk=[γ1kγ2kγ3kγ4k]T0≤γik≤1,i=1,2,3,4

(8)

式中:γik=0,表示执行器无异常;0<γik<1,表示执行器部分异常;γik=1,表示执行器完全失效。

当三旋翼无人机执行器发生异常行为时,式(6)可以改写为

xk+1=Akxk+U1(k)-U2(k)γk+w1k

(9)

为了更好地得到γk的真实估计,定义随机发生的执行器异常行为满足

γk+1=γk+w2k

(10)

式中,w2k是均值为零、协方差为Q2k的不相关高斯随机向量。

综上所述,三旋翼无人机执行器异常行为下的系统离散状态空间方程为

xk+1=Akxk+U1(k)-U2(k)γk+w1k

(11)

γk+1=γk+w2k

(12)

yk+1=Ckxk+1+vk+1。

(13)

2 两级卡尔曼滤波器设计

基于卡尔曼滤波算法,分别对执行器异常行为以及系统状态进行滤波估计[17]。

设计执行器异常行为下状态估计子滤波器为

(14)

(15)

(16)

(17)

(18)

以卡尔曼滤波算法为基础设计三旋翼无人机执行器异常行为估计子滤波器以及状态估计滤波器,设计两个滤波器之后,由算式[18-19]

xk+1|k=Akxk|k+U1(k)+U2(k)γk|k

(19)

可以得到有关状态估计误差的子滤波器,通过耦合方程将异常行为估计滤波器代入式(19)可以将状态误差估计子滤波器表示出来,具体的表示形式如下。

在采用两级卡尔曼滤波器对执行器异常行为检测诊断之前,首先利用卡尔曼滤波算法对没有异常行为发生时的系统状态进行分析,得到没有异常行为时系统的最优估计之后,将异常行为和系统状态进行隔离分析,过程表述为

xk+1|k+1=xk+1|k+Kk+1[yk+1-Ckxk+1|k]
↓
xk+1|k=Akxk|k+U1(k)+U2(k)γk|k

(20)

然后利用协方差矩阵将包含异常行为以及系统状态误差的最优估计表示出来,但是这种表示方式的计算量以及阶数都较大,为解决这个问题,将异常行为以及系统状态进行隔离分析,同时,将两者之间的耦合项单独分析,这样一来计算量不仅大大减小,而且减少了耦合项的影响,滤波的结果更加直观准确,更容易观察到不同异常行为类型的发生对系统状态的影响。以卡尔曼滤波算法为基础设计三旋翼无人机执行器异常行为估计子滤波器以及状态估计滤波器,可以得到有关状态估计误差的子滤波器,通过耦合方程将异常行为估计滤波器代入式(20)可以将状态误差估计子滤波器表示出来,具体的表示形式如下。

设计执行器未发生异常行为时状态误差估计子滤波器为

(21)

(22)

(23)

(24)

(25)

状态估计子滤波器残差及其协方差分别为

(26)

(27)

相关耦合方程定义为

(28)

Wk=AkVk|k-U2(k)

(29)

(30)

Hk+1|k=Ck+1Vk+1|k。

(31)

由以上两级卡尔曼滤波器可以得到状态的最优估计及其协方差估计为

(32)

(33)

由上述分析可以得出,两级卡尔曼滤波算法对卡尔曼滤波算法会引起滤波结果发散等不足的改进体现在:

1) 建立相关模型之前,首先对随机发生的异常行为建模分析,得到随机发生的异常行为的相关方程,然后将得到的异常行为模型应用到系统状态方程中;

2) 首先采用卡尔曼滤波算法得到异常行为的最优估计,然后分析耦合项的影响,通过耦合方程将异常行为最优估计代入到系统状态的最优估计中,这在一定程度上减少了异常行为滤波结果的影响,同时,对系统状态和异常行为分别进行运算,降低模型复杂度,减小计算量;

3) 通过耦合方程设计耦合器来对系统状态进行补偿,使得系统最优估计及其误差协方差的误差减小,对滤波器结果的发散可以起到抑制作用。

由上述内容可以得到两级卡尔曼滤波算法从减小与实际应用的偏差、对模型建立过程简化以及减小计算量3个方面实现对卡尔曼滤波器易发散等不足的改进。

两级卡尔曼滤波算法设计的公式阐述了滤波的整体过程,滤波器方程分为两组:时间更新方程以及量测更新方程,时间更新方程由(k+1|k)部分表示,即通过提前一步预测状态和误差协方差来获得先验估计;而由(k+1|k+1)部分表示量测更新方程,即通过得到的先验估计的反馈来获得后验估计。前者用于预测,后者用于校正。

3 数值仿真

三旋翼无人机上实现的系数矩阵设计为

(34)

(35)

针对不同执行器异常得到的仿真结果分别如图2-图6所示。

图2 异常行为诊断曲线:单执行器异常(γ4=0.3)Fig.2 Abnormal behavior diagnosis curve with single abnormal actuator (γ4=0.3)

由图2和图3可以看出,当三旋翼无人机执行器未出现异常行为时,γk的估计值收敛于真实值。第10 s时,偏航通道分别发生0.3倍和0.8倍失效,γ4的真实值发生突变,其估计值在两阶卡尔曼滤波算法的作用下,快速收敛于真实值,达到异常行为诊断的目的。

图3 异常行为诊断曲线:单执行器异常(γ4=0.8)Fig.3 Abnormal behavior diagnosis curve with single abnormal actuator(γ4=0.8)

从图4和图5可以看出,第10 s时,滚转通道和偏航通道同时分别发生0.3倍和0.8倍失效,γ2和γ3的真实值发生突变,其估计值在两阶卡尔曼滤波算法的作用下快速收敛于真实值,达到异常行为诊断的目的。

图4 异常行为诊断曲线:部分执行器发生异常(γ2=γ3=0.3)Fig.4 Abnormal behavior diagnosis curve with partial abnormal actuators(γ2=γ3=0.3)

图5 异常行为诊断曲线:部分执行器发生异常(γ2=γ3=0.8)Fig.5 Abnormal behavior diagnosis curve with partial abnormal actuators(γ2=γ3=0.8)

由图6可以看出,第10 s时,三旋翼无人机每个执行器均发生0.8倍失效异常,γ1的真实值发生突变,其估计值在两阶卡尔曼滤波算法的作用下快速收敛于真实值,能够实现异常行为诊断,但收敛时间较单执行器或2个执行器异常时所用时间更多。

图6 异常行为诊断曲线:全部执行器异常(γ1=0.8)Fig.6 Abnormal behavior diagnosis curve with all actuators being abnormal (γ1=0.8)

综上所述,应用两级卡尔曼滤波算法能准确地检测出三旋翼无人机执行器发生的异常行为,同时可以分析出各个执行器发生异常行为后对系统的影响程度。

4 结束语

本文针对三旋翼无人机执行器异常行为诊断问题,基于牛顿-欧拉方法对三旋翼无人机异常行为动力学模型进行了分析,在此基础上设计了基于两级卡尔曼滤波器的异常行为诊断算法实现了对三旋翼无人机执行器异常行为的估计。通过数值仿真对执行器几种不同情形的异常行为分别进行了验证,结果表明,所设计算法能够较好地实现对三旋翼无人机执行器异常行为的诊断。未来将对基于异常行为诊断的容错控制算法进行研究,以实现无人机执行器异常下的稳定控制。