基于姿態(tài)角估計的四旋翼飛行器多傳感器故障診斷方法

王日俊，曾志強，段能全，黨長營，杜文華，王俊元

（中北大學 機械工程學院，太原 030051）

四旋翼飛行器是一種結構簡單的無人飛行器，具有靈活起降、自主飛行以及穩(wěn)定好等特點，在軍事與民用領域存在巨大的應用潛力，因而受到各國研究人員的廣泛關注和重視[1-2]。由多種傳感器組成的慣性測量單元（Inertial Measurement Units, IMU）是其執(zhí)行飛行任務的重要環(huán)節(jié)，對于飛行器的姿態(tài)運動控制和自主飛行控制起著關鍵作用[3]。但是，惡劣的機載環(huán)境存在著溫度變化、機械振動等因素極易導致IMU傳感器發(fā)生故障[4-5]。傳感器故障的一旦發(fā)生，不僅會對飛行的安全性、穩(wěn)定性造成嚴重影響，更會導致任務執(zhí)行失敗，甚至是造成地面人員的傷亡。因此，開展四旋翼飛行器傳感器故障診斷技術的研究對于提高飛行器的安全性和可靠性具有重要的意義與價值。

目前，針對四旋翼飛行器故障診斷技術的研究主要集中于執(zhí)行器故障，而應用于傳感器故障診斷的研究成果仍然有限[6]?；谌钟^測器的傳感器故障診斷方法提出不久，多種觀測器或估計器方法，如基于線性參數(shù)變化自適應觀測器[7]、集員估計器[8]，非線性狀態(tài)觀測器[9]和降階非線性觀測器[10]等已被相關研究人員逐漸應用于四旋翼飛行器的傳感器故障診斷方法的研究工作中。此外，基于擴展卡爾曼濾波（Extended Kalman filter, EKF）以及強跟蹤卡爾曼濾波（Strong Tracking Kalman filter, STKF）的四旋翼飛行器狀態(tài)與傳感器故障偏差估計方法也取得了一定的研究成果[11-12]。然而，由于需引入額外的系統(tǒng)狀態(tài)變量來描述傳感器的故障，使其難以滿足有限傳感器測量的可觀測條件，難以對參數(shù)估計的穩(wěn)定性進行分析。在包括上述的研究工作在內的四旋翼飛行器的故障診斷方法均是在不考慮系統(tǒng)模型誤差，且假設姿態(tài)角可直接測量得到的前提下進行的。然而，在實際的應用中，系統(tǒng)模型中的升力、轉動力矩等無法測量得到，滾轉角和俯仰角通常也是通過機載IMU信息估計得到的。

針對上述問題，本文在考慮四旋翼飛行器動力學模型誤差的基礎上，提出一種四旋翼飛行器的傳感器多故障診斷及故障偏差值估計方法。該方法僅利用加速度信息對姿態(tài)角進行估計，通過基于估計姿態(tài)角的故障觀測器對多傳感器故障進行檢測并隔離，構建非線性自適應故障偏差觀測器，同時對多個故障偏差值進行估計，從而為飛行器的安全、可靠飛行提供有力的保障。

1 系統(tǒng)描述

1.1 四旋翼飛行器建模

圖1為四旋翼飛行器的結構示意圖。取四旋翼飛行器的質心O作為機體坐標系OxByBzB的原點。慣性坐標系OxEyEzE的原點與飛行器起飛位置時OxByBzB的原點重合。4個執(zhí)行器Mi（i=1, 2, 3, 4）均由直流電機和旋翼組成，其中，執(zhí)行器 M2、M4按順時針方向旋轉，執(zhí)行器M1、M3按逆時針方向旋轉。升力Fi（i=1,2, 3, 4）由4個執(zhí)行器提供。飛行器通過調整4個執(zhí)行器之間的轉速差來完成在各個方向上的姿態(tài)轉動控制。

圖1 四旋翼飛行器的結構Fig.1 Structure of quad-rotor aircraft

在四旋翼飛行器的實際應用中，包括旋翼電機提供的升力U，作用于飛行器繞x、y、z軸的轉動力矩Tφ、Tθ、Tφ等在內的系統(tǒng)輸入變量均無法精確測量得到，通常認為它們與電機速度的平方成正比。而電機轉速則通過PWM指令以及驅動電壓值估計得到。這在很大程度上造成了建模的不確定性?？紤]上述因素，結合牛頓-歐拉運動學方程得到四旋翼飛行器動力學模型如式(1)所示。

其中，m表示四旋翼飛行器的質量，g表示重力加速度，pE、vE分別表示慣性坐標系下的位置和速度，歐拉角角速度Jz分別表示飛行器繞 x、y、z軸的轉動慣量；U表示由4個執(zhí)行器提供的總升力；dBcv表示作用于機體的阻力，dc為阻力系數(shù)，Bv為機體坐標系下沿x、y、z方向上的速度，且為機體坐標系到慣性坐標系的轉化矩陣。

假設非線性科氏力忽略不計，則四旋翼飛行器在機體坐標系下的速度可表示為[13]：

其中，φ和 ub分別表示PWM信號和驅動電壓值。

1.2 傳感器偏差故障建模

由于飛行器重量與體積的限制，質量輕、體積小的MEMS傳感器已成為機載IMU慣性測量器件的首選，如三軸陀螺儀、三軸加速度計、磁力計等。它們在飛控系統(tǒng)的導航信息獲取和姿態(tài)運動控制中起著關鍵性作用。偏差故障是一類常見且不易檢測到的IMU傳感器軟故障，一旦發(fā)生會導致飛控系統(tǒng)無法正常發(fā)揮作用。在實際偏差故障發(fā)生時，加速度計與陀螺儀傳感器的模型可描述為：

其中，ya(t)表示加速度計的實測值，yω(t)表示陀螺儀的實測值； ba表示加速度計的故障偏差值，且表示陀螺儀的故障偏差值，且表示無偏差故障時陀螺儀的測量值，且表示無偏差故障與測量噪聲時加速度計測量值，且：

在實際情況下，通常故障偏差值ab、ωb隨時間的變化較為緩慢，在短持續(xù)時間內ab、ωb的變化量也很小[14]。因此，可認為故障偏差值在短持續(xù)時間內是恒定的，則可做如下假設。

假設1：加速度計和陀螺儀的故障偏差值ab和ωb均為有界的常數(shù)，即：

此外，系統(tǒng)的輸出可表示為：

其中，()tψ為慣性坐標系下的飛行器的偏航角，它與飛行器的位置均可由GPS單元和磁力計測量計算得到。

2 偏差故障檢測與診斷（Bias Fault Detection and Diagnosis，BFDD）系統(tǒng)設計

為了實現(xiàn)對多個同時發(fā)生的傳感器故障進行檢測與隔離，并通過第1節(jié)中式(1)(5)(6)(12)所描述的非線性系統(tǒng)模型對故障偏差值進行估計，本文結合滾轉角和俯仰角無法直接測量得到的實際應用情況，考慮到傳感器的六個分量可能同時出現(xiàn)的多個偏差故障，所設計的四旋翼飛行器傳感器偏差故障檢測與診斷系統(tǒng)結構如圖2所示。

整個BFDD系統(tǒng)由偏差故障的檢測與隔離、故障偏差值的估計兩部分構成。BFDD系統(tǒng)的輸入包括控制器輸出、傳感器測量值以及滾轉角/俯仰角估計值。系統(tǒng)通過加速度計故障觀測器和陀螺儀故障觀測器對傳感器狀態(tài)進行實時估計，一旦傳感器的故障被檢測到并隔離，便會激活與之對應的非線性自適應故障偏差觀測器完成對故障偏差值的估計，得到的偏差估計值可有助于提升飛行器控制系統(tǒng)的容錯能力，從而保證飛行器的穩(wěn)定性與安全性。

圖2 偏差故障檢測與診斷系統(tǒng)結構Fig.2 Structure of bias fault detection and diagnosis system

3 基于滑模觀測器的姿態(tài)角估計

將式(5)代入式(1)可得：

其中，yu和yv分別表示飛行器在機體坐標系下x方向和 y方向上的平移加速度測量值。分別對式(13)和式(14)進行求導運算，并結合式(2)有：

將式(15)和(16)中出現(xiàn)的正弦項作為系統(tǒng)動力學的未知輸入。應用文獻[15]中所提出的基于未知輸入估計的滑模觀測器方法，則有：

且參數(shù)uρ、vρ滿足如下條件：

其中，uξ、vξ為小的正常數(shù)。利用式(15)～(18)，得到加速度計動態(tài)估計誤差為：

選取Lyapunov函數(shù)為：

對式(25)求導，并結合式(19)得到：

結合式(21)有：

式(28)提供了滑模運動的可達性，在滑模運動中，由于式(17)中的信號uζ上的有限切換頻率，我們有通常，俯仰角θ的變化比信號uζ的開關頻率慢得多，有：

其中，ueqζ為保持滑模運動所需的等效輸出誤差注入信號，并表示在式(19)中定義的非連續(xù)分量的平均行為?？梢酝ㄟ^使用低通濾波器來得到，即：

其中δ為一正常數(shù)。因此，未知的正弦項可估計為：

同樣地，

其中，veqζ為保持滑模運動所需的等效輸出誤差注入信號。因此，通過使用式(31)(32)，在沒有加速度計故障的情況下，可以產生對滾轉角θ和俯仰角度φ的估計值為：

4 傳感器的故障觀測器設計

將如式(5)(6)所描述的傳感器偏差故障模型代入四旋翼飛行器動力學模型有：

可以看出，加速度計偏差故障只出現(xiàn)在系統(tǒng)的位置和速度的動力學方程中，陀螺儀偏差故障只出現(xiàn)在系統(tǒng)的歐拉角和角速率動力學方程中。因此，可將這兩類傳感器偏差故障分別進行故障的檢測與診斷。

4.1 加速度計故障觀測器

根據式(35)，得到四旋翼飛行器的質心運動方程為：

設計如下加速度計故障觀測器：

且漸進穩(wěn)定。{A, C}完全能觀測。

當att＜時，加速度計并未發(fā)生故障，在有限時間內俯仰角和俯仰角的估計值與實際值非常接近，也就意味著Δ0→f，因此有：

利用三角不等式，并結合式(38)(39)有，加速度計輸出估計誤差滿足：

其中，jC表示矩陣 C的第 j列（j=1,2,3），常數(shù)均大于0，且因此，得到加速度計故障檢測與隔離的閾值為：

4.2 陀螺儀故障觀測器

在假設模型誤差有界的前提下，陀螺儀輸出估計誤差滿足：

當ttω＞時，陀螺儀發(fā)生故障，則陀螺儀輸出的估計誤差滿足：

5 傳感器故障偏差觀測器

5.1 加速度計故障偏差觀測器

設計用以估計加速度計故障偏差值的非線性自適應觀測器為：

當加速度計傳感器發(fā)生偏差故障，并在Ta＞ta時刻被檢測到，則慣性坐標系下的速度估計誤差為：

利用三角不等式有：

其中，μ0和β0均為正常數(shù)，且為狀態(tài)估計誤差初始值的上界，即

5.2 陀螺儀故障偏差觀測器

根據式(33)～(35)，基于Lyapunov方法設計陀螺儀故障偏差觀測器為：

假設陀螺儀傳感器在tω時發(fā)生偏差故障，在Tω時刻檢測到故障，且定義為了保證參數(shù)的收斂性，對于在任意時，濾波器需滿足如下條件：

其中，常數(shù)α1＞0，α0＞0和T0＞0。I3表示33×單位矩陣。定義歐拉角估計誤差陀螺儀故障偏差的估計為

若陀螺儀在tω時刻發(fā)生偏差故障，在Tω時刻故障被檢測到，在任意t＞時，在滿足式(55)給定的條件下，則由式(54)表示的偏差故障觀測器中所有信號均有界，且陀螺儀故障偏差的估計值收斂于零[16]。

6 實驗研究

整個實驗平臺系統(tǒng)主要由課題組設計開發(fā)的四旋翼飛行器、地面站以及Vicon運動捕捉系統(tǒng)組成。搭建的室內實驗平臺系統(tǒng)結構框圖如圖 3所示。Vicon運動捕捉系統(tǒng)以 1 Hz的采樣頻率獲取四旋翼飛行器飛行過程中的姿態(tài)和位置數(shù)據信息，并通過 TCP/IP協(xié)議將數(shù)據傳輸?shù)降孛嬲荆鳛閷嶒炛凶藨B(tài)和位置信息的參考值。地面站通過遠距離射頻模塊XTend與飛行器相連接，從而實現(xiàn)數(shù)據快速可靠的傳輸與參數(shù)的在線調整。四旋翼飛行器的基本參數(shù)如表1所示。

圖3 實驗平臺系統(tǒng)結構框圖Fig.3 Structure block diagram of experimental system

表1 四旋翼飛行器參數(shù)Tab.1 Parameters of quadrotor aircraft

在室內條件下，四旋翼飛行器按照既定半徑為1m的圓形軌跡飛行過程中，來驗證本文所設計的故障診斷方法的有效性。其中，用于估計姿態(tài)角的滑模觀測器參數(shù)取值分別為：故障觀測器增益矩陣的取值分別為：式(4)中表示的模型誤差上界分別取和通過加速度計偏差故障、陀螺儀偏差故障以及加速度計和陀螺儀同時發(fā)生偏差故障的實驗來驗證本方法在單個和多個故障情況下的有效性。

6.1 加速度計偏差故障實驗

在t = 30 s時，加速度計引入人為設定的故障偏差ba= [?0.45, 0.4, 1.2]T(m/s2)，姿態(tài)角的前后估計結果如圖4所示。由圖4可知，在加速度計偏差故障發(fā)生前后系統(tǒng)均能較為精確的實現(xiàn)對姿態(tài)角的估計。值得注意的是，在30～32 s之間，姿態(tài)角的估計值與測量值之間誤差僅出現(xiàn)了短暫的波動，這與本文所提出的姿態(tài)估計算法僅利用機體坐標系下x和y方向的加速度信號有關。

圖4 引入ba后的滾轉角和俯仰角估計曲線Fig.4 Estimation of roll angle and pitch angle with ba

圖5 引入ba后的故障診斷殘差Fig.5 The diagnostic residuals with ba

在引入故障偏差ba后，所設計故障觀測器對故障殘差診斷的實驗結果如圖5所示。圖5(a)為加速度計故障殘差診斷結果，加速度計故障觀測器在引入故障偏差ba后，診斷殘差超過檢測閾值，加速度計偏差故障被檢測到。圖5(b)為陀螺儀故障殘差診斷結果，診斷殘差并未超過檢測閾值，陀螺儀無偏差故障。

在檢測到加速度計偏差故障之后，便會激活與之對應的故障偏差觀測器，估計故障偏差的實驗結果如圖6所示。在30 s處發(fā)生偏差故障ba后，加速度計故障偏差觀測器能夠快速估計得到故障偏差ba，且估計誤差有界。顯然，所設計的加速度計故障偏差觀測器能夠準確估計并跟蹤故障偏差ba，其有效性得以驗證。

圖6 加速度計故障偏差估計曲線Fig.6 Acceleration bias fault estimation

6.2 陀螺儀偏差故障實驗

在 t = 30 s時，陀螺儀引入人為設定的故障偏差bω= [5, 7, 10]T(°/s)，姿態(tài)角的前后估計結果如圖7所示。

圖7 引入bω后的滾轉角和俯仰角估計曲線Fig.7 Estimation of roll angle and pitch angle with bω

由圖7可知，在故障發(fā)生前后系統(tǒng)均能較為精確的實現(xiàn)對姿態(tài)角的估計。與加速度計故障情況不同的是，姿態(tài)角的估計值與測量值之間誤差并未出現(xiàn)短暫的波動。

在引入故障偏差bω后，所設計故障觀測器對故障殘差診斷的實驗結果如圖8所示。圖8(a)為加速度計故障殘差診斷結果，診斷殘差未超過檢測閾值，加速度計無偏差故障。圖8(b)為陀螺儀故障殘差診斷結果，陀螺儀故障觀測器在引入故障偏差bω后，診斷殘差超過檢測閾值，陀螺儀偏差故障被檢測到。

圖8 引入bω后的故障診斷殘差Fig.8 The diagnostic residuals with bω

在檢測到陀螺儀偏差故障之后，便會激活與之對應的偏差觀測器，估計故障偏差值的實驗結果如圖 9所示。在30 s處發(fā)生偏差故障bω后，陀螺儀故障偏差觀測器能夠在10s之后完成對故障偏差ba的估計，且接近給定值。所設計陀螺儀故障偏差觀測器能夠準確估計并跟蹤故障偏差bω，其有效性得以驗證。

圖9 陀螺儀故障偏差估計曲線Fig.9 Gyroscope bias fault estimation

6.3 多故障實驗

在時間t = 30 s時，同時引入加速度計故障和陀螺儀偏差故障，用以驗證同時對多故障進行診斷與估計的能力。引入的偏差故障大小分別為ba= [5.5, -4.5, 1.2]T(m/s2)和bω= [4.5, 3, 5]T(°/s)。姿態(tài)角的估計結果如圖10所示。

圖10 多故障時滾轉角和俯仰角估計曲線Fig.10 Estimation of roll angle and pitch angle with multiple simultaneous faults

加速度計和陀螺儀同時發(fā)生偏差故障，通過故障觀測器對故障殘差進行診斷的實驗結果如圖11所示。由圖可知，在多故障發(fā)生后，診斷殘差均在較短時間內超過各自的檢測閾值，從而檢測到同時發(fā)生的偏差故障。在多故障條件下，驗證了本文方法的對多故障檢測的有效性。

圖11 多故障時故障診斷殘差Fig.11 Diagnostic residuals with multiple simultaneous faults

在檢測到加速度計和陀螺儀偏差故障之后，便會激活與之對應的偏差觀測器，在多故障條件下，估計故障偏差的實驗結果如圖12所示。圖12(a)為估計加速度計故障偏差的實驗結果，圖12(b)為估計陀螺儀故障偏差的實驗結果。由圖可知，本文設計的加速度計故障偏差觀測器和陀螺儀故障偏差觀測器均能夠在5 s之內準確估計和跟蹤各自的故障偏差值，且估計誤差有界。

圖12 多故障時故障偏差估計曲線Fig.12 Bias fault estimation in the case of multiple simultaneous faults

7 結 論

為了提高四旋翼飛行器的容錯能力，保證其安全可靠地完成飛行作業(yè)任務，本文針對四旋翼飛行器多傳感器偏差故障的診斷問題，在飛行器姿態(tài)角無法直接測量得到的前提下，提出一種基于姿態(tài)角估計的四旋翼飛行器多傳感器偏差故障診斷方法，并構建出適用于四旋翼飛行器的多傳感器偏差故障診斷系統(tǒng)。數(shù)值仿真和實際飛行實驗結果表明，所構建的故障診斷系統(tǒng)可快速地實現(xiàn)對多傳感器偏差故障的檢測與隔離，對故障偏差值的準確估計與跟蹤，進而有效地提高四旋翼飛行器的可靠性與安全性。