王日俊，白 越，曾志強(qiáng)，段能全，黨長營，杜文華，王俊元

(1.中北大學(xué) 機(jī)械工程學(xué)院，太原 030051； 2.中國科學(xué)院 長春光學(xué)精密機(jī)械與物理研究所，長春 130033)

0 引言

四旋翼無人飛行器具有結(jié)構(gòu)簡單、穩(wěn)定性高以及起降靈活等特點(diǎn)，在軍事和民用領(lǐng)域得到了極大關(guān)注與廣泛應(yīng)用[1-2]。搭載于飛行器上的慣性測量單元(Inertial Measurement Unit, IMU)在其導(dǎo)航信息獲取與姿態(tài)運(yùn)動(dòng)控制中起著至關(guān)重要的作用，是保證飛行器安全可靠飛行、執(zhí)行各類飛行任務(wù)的前提[3]。然而，機(jī)載環(huán)境的溫度變化、機(jī)械振動(dòng)等因素的存在使得IMU傳感器元件極易損傷或失效，由此導(dǎo)致的傳感器故障時(shí)有發(fā)生[4-5]。傳感器一旦發(fā)生故障不僅會嚴(yán)重影響飛行的安全性和可靠性，導(dǎo)致飛行任務(wù)的失敗，甚至?xí)＜暗孛嫒藛T的生命安全，因此對于飛行器傳感器故障檢測與診斷技術(shù)的研究就成為了提高飛行器的安全性和可靠性的迫切任務(wù)。

目前大多數(shù)應(yīng)用于四旋翼無人飛行器的故障診斷方法都只涉及執(zhí)行器故障，而傳感器故障檢測和隔離(Fault Detection and Isolation, FDI)的研究成果仍然有限[6]。在文獻(xiàn)[7]提出傳感器故障全局觀測器方法之后，相關(guān)研究人員逐漸將多種觀測器或估計(jì)器方法應(yīng)用于四旋翼無人飛行器的傳感器故障檢測與隔離的研究工作中，包括基于線性參數(shù)變化自適應(yīng)觀測器[8]、集員估計(jì)器[9]，非線性狀態(tài)觀測器[10]和降階非線性觀測器[11]等。Freddi等[12]采用基于全姿態(tài)測量的Thau觀測器實(shí)現(xiàn)了加速度計(jì)偏差故障的檢測。在假設(shè)四旋翼無人飛行器的平移加速度為零時(shí)，Berbra等[13]提出了一種陀螺儀、加速度計(jì)和磁力計(jì)故障的檢測方法。此外，利用卡爾曼濾波(Kalman Filter, KF)方法同時(shí)估計(jì)四旋翼飛行器狀態(tài)和傳感器偏差，也取得了一定的效果[14-15]；然而，對非線性系統(tǒng)參數(shù)估計(jì)的擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF)或無跡卡爾曼濾波(Unscented Kalman Filter, UKF)的穩(wěn)定性分析是非常困難的，引入的額外的系統(tǒng)狀態(tài)變量代表未知的傳感器故障參數(shù)，使其難以滿足有限的傳感器測量的可觀測條件。

本文針對四旋翼無人飛行器的加速度計(jì)和陀螺儀傳感器的多故障檢測問題，提出一種基于非線性自適應(yīng)觀測器的多傳感器故障檢測與故障偏差估計(jì)方法。該方法利用非線性故障觀測器實(shí)現(xiàn)多故障的檢測與隔離，基于非線性自適應(yīng)觀測器同時(shí)對多個(gè)未知故障的大小進(jìn)行估計(jì)，從而提高飛行器的容錯(cuò)能力，保證飛行的安全性和可靠性。

1 系統(tǒng)描述

四旋翼無人飛行器的結(jié)構(gòu)如圖1所示。

圖1 四旋翼無人飛行器的結(jié)構(gòu)

圖中，OxByBzB為機(jī)體坐標(biāo)系，原點(diǎn)O取飛行器的質(zhì)心。OxEyEzE為慣性坐標(biāo)系，慣性坐標(biāo)系的原點(diǎn)與飛行器起飛位置時(shí)機(jī)體坐標(biāo)系的原點(diǎn)重合。Mi(i=1,2,…,4)為4組由直流電機(jī)和旋翼組成的執(zhí)行機(jī)構(gòu)，其中，M2、M4順時(shí)針旋轉(zhuǎn)，M1、M3逆時(shí)針旋轉(zhuǎn)。Fi(i=1,2,…,4)為4個(gè)執(zhí)行機(jī)構(gòu)提供的升力。飛行器通過調(diào)整4個(gè)旋翼之間的轉(zhuǎn)速差來完成各個(gè)方向上的姿態(tài)轉(zhuǎn)動(dòng)控制。

由牛頓-歐拉運(yùn)動(dòng)學(xué)方程得到的四旋翼無人器動(dòng)力學(xué)模型[16]可描述為：

(1)

(2)

Rη(φ,θ)表示機(jī)體坐標(biāo)系下旋轉(zhuǎn)速度到姿態(tài)角的角速度的轉(zhuǎn)換矩陣，且:

(3)

四旋翼無人飛行器通常搭載有成本低、質(zhì)量輕的微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System, MEMS)傳感器如3軸陀螺儀、加速度計(jì)等作為其慣性測量單元(Inertial Measurement Unit, IMU)。這些傳感器在四旋翼無人飛行器的導(dǎo)航信息獲取與姿態(tài)運(yùn)動(dòng)控制中起著至關(guān)重要的作用。通常，加速度計(jì)與陀螺儀的測量值可表示為：

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

(4)

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

(5)

其中:ya(t)為實(shí)際加速度計(jì)測量值，yω(t)為實(shí)際陀螺儀測量值，ba為加速度計(jì)測量中的故障偏差值，bω為陀螺儀測量中的故障偏差值，na(t)、nω(t)分別為加速度計(jì)和陀螺儀的測量噪聲。a(t)表示無偏差故障與測量噪聲時(shí)加速度計(jì)測量值，且:

(6)

ta、tω分別為加速度計(jì)和陀螺儀故障的發(fā)生時(shí)刻，λa(t-ta)，λω(t-tω)是關(guān)于故障時(shí)間的階躍函數(shù)，即：

(7)

(8)

在實(shí)際的應(yīng)用中，當(dāng)加速度計(jì)和陀螺儀發(fā)生故障后，故障偏差值ba、bω的大小會隨時(shí)間緩慢地變化，但在短時(shí)間內(nèi)的偏差值的變化量很小[17]，因此，在考慮的短時(shí)間持續(xù)時(shí)間內(nèi)可認(rèn)為故障偏差值是恒定的。于是有如下假設(shè)。

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

(9)

(10)

假設(shè)2 在式(4)和(5)中定義的測量噪聲na(t)、nω(t)均為有界的零均值信號，且E(na(t))=0，E(nω(t))=0，E(·)為期望算子。

另外，假設(shè)慣性坐標(biāo)系下四旋翼無人飛行器的位置和偏航角可測量得到，則整個(gè)系統(tǒng)模型的輸出方程可表示為：

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

(11)

其中:dp(t)為零均值測量噪聲，且E(dp(t))=0。

2 多傳感器故障檢測與診斷系統(tǒng)

將如式(5)和(6)所示的傳感器模型代入四旋翼無人飛行器動(dòng)力學(xué)模型有：

(12)

從式(12)可知，加速度計(jì)測量中的偏差只出現(xiàn)在表示位置和速度的動(dòng)力學(xué)方程中，而陀螺儀測量中的偏差只出現(xiàn)在表示歐拉角和角速率動(dòng)力學(xué)方程中，因此，傳感器測量中的偏差的影響可以被視為虛擬的執(zhí)行器故障。此外，在四旋翼無人飛行器的狀態(tài)方程中，兩種傳感器故障互不影響，相互之間不存在耦合現(xiàn)象。

根據(jù)上述分析，本文所設(shè)計(jì)的四旋翼無人飛行器的多傳感器故障檢測與診斷系統(tǒng)(Fault Detection and Diagnosis，F(xiàn)DD)系統(tǒng)結(jié)構(gòu)如圖2所示。根據(jù)FDD系統(tǒng)實(shí)現(xiàn)的功能可將結(jié)構(gòu)分為故障檢測與隔離FDI和故障偏差值估計(jì)(Fault Bias Estimation, FBE)兩部分。圖中控制器的輸出信號與傳感器的測量值作為FDI的輸入。通常情況下，兩個(gè)FDI分別用來實(shí)現(xiàn)加速度計(jì)和陀螺儀測量過程中故障的檢測與隔離，一旦其中某一故障被檢測到并隔離，便會激活與之對應(yīng)的FEB完成對故障偏差值的估計(jì)，從而增強(qiáng)飛行器控制系統(tǒng)的容錯(cuò)能力，保證飛行器的穩(wěn)定性與安全性。

圖2 多傳感器故障檢測與診斷系統(tǒng)結(jié)構(gòu)

2.1 傳感器的故障觀測器設(shè)計(jì)

2.1.1 加速度計(jì)故障觀測器

由式(12)可知，四旋翼無人飛行器的質(zhì)心運(yùn)動(dòng)方程可表示為：

(13)

設(shè)計(jì)如下加速度計(jì)故障觀測器：

(14)

(15)

(16)

(17)

2.1.2 陀螺儀故障觀測器

根據(jù)式(12)，設(shè)計(jì)如下陀螺儀故障觀測器：

(18)

(19)

當(dāng)t

hω(t)+eω(t)

(20)

(21)

2.2 傳感器故障偏差的非線性自適應(yīng)觀測器設(shè)計(jì)

2.2.1 加速度計(jì)故障偏差觀測器

根據(jù)式(12)，用以估計(jì)加速度計(jì)故障偏差值的非線性自適應(yīng)觀測器可表示為：

(22)

(23)

其中Δ是一個(gè)對稱正定的學(xué)習(xí)率矩陣。

關(guān)于所設(shè)計(jì)參數(shù)自適應(yīng)律的穩(wěn)定性存在如下定理。

定理1 假設(shè)加速度計(jì)傳感器發(fā)生故障在ta時(shí)，在Ta時(shí)刻檢測到故障，且Ta>ta>0，那么，對于在任意t>Ta時(shí)，如果存在常數(shù)α1≥α0>0和T0>0，有：

(24)

(25)

(26)

(27)

(28)

對式(27)進(jìn)行期望值的計(jì)算，結(jié)合E(na(t))=0和E(dp(t))=0，可得：

(29)

(30)

2.2.2 陀螺儀故障偏差觀測器

根據(jù)式(12)，基于Lyapunov方法設(shè)計(jì)用以估計(jì)陀螺儀故障偏差的自適應(yīng)觀測器為：

(31)

為了保證參數(shù)的收斂性，Rη(φ,θ)需滿足如下條件：

(32)

證明 如式(31)所示的自適應(yīng)觀測器的狀態(tài)與參數(shù)的估計(jì)誤差可表示為：

(33)

由假設(shè)2有，nω有界，則擾動(dòng)項(xiàng)Rη(φ,θ)nω也有界，顯然式(33)中的信號均有界。對式(33)進(jìn)行期望值的計(jì)算，結(jié)合E(nω(t))=0，可得：

(34)

(35)

3 實(shí)驗(yàn)研究

搭建的室內(nèi)實(shí)驗(yàn)平臺系統(tǒng)結(jié)構(gòu)如圖3所示。整個(gè)系統(tǒng)由課題組研制的四旋翼無人飛行器、Vicon運(yùn)動(dòng)捕捉系統(tǒng)以及地面站三部分組成。在飛行過程中，四旋翼無人飛行器的姿態(tài)信息和位置信息通過Vicon運(yùn)動(dòng)捕捉系統(tǒng)獲取，采樣頻率1 Hz，通過TCP/IP協(xié)議實(shí)現(xiàn)與地面站的通信。地面站通過XTend遠(yuǎn)距離射頻模塊連接四旋翼無人飛行器，實(shí)現(xiàn)數(shù)據(jù)快速可靠的傳輸與參數(shù)的在線調(diào)整。

圖3 室內(nèi)實(shí)驗(yàn)平臺系統(tǒng)結(jié)構(gòu)框圖

圖4 故障設(shè)置時(shí)序

3.1 加速度計(jì)故障實(shí)驗(yàn)

圖5 加速度計(jì)故障檢測

圖6 加速度計(jì)故障偏差估計(jì)

3.2 陀螺儀故障實(shí)驗(yàn)

圖7 陀螺儀故障檢測

3.3 多傳感器故障實(shí)驗(yàn)

在檢測加速度計(jì)和陀螺儀同時(shí)發(fā)生故障后，對應(yīng)的偏差觀測器對故障偏差值的估計(jì)結(jié)果分別如圖9和圖10所示。其中，圖9為對加速度計(jì)故障偏差的估計(jì)結(jié)果，圖10為對陀螺儀故障偏差的估計(jì)結(jié)果?？梢钥闯觯疚奶岢龅墓收显\斷方法能夠?qū)崿F(xiàn)對多傳感器故障偏差的快速估計(jì)與有效跟蹤。

圖8 陀螺儀故障偏差估計(jì)

圖9 同時(shí)故障時(shí)加速度計(jì)故障偏差估計(jì)

圖10 同時(shí)故障時(shí)陀螺儀故障偏差估計(jì)

為了驗(yàn)證多傳感器故障條件下飛行器控制的穩(wěn)定性，設(shè)定飛行器的飛行軌跡為矩形，在無故障條件進(jìn)行飛行，得到的姿態(tài)角曲線如圖11中的實(shí)線所示。在t=30 s時(shí)，在加速計(jì)和陀螺儀測量過程中引入固定偏差故障，得到有故障條件下的姿態(tài)角曲線如圖11中的虛線所示。

從圖中可以看出，相對于無故障發(fā)生時(shí)，在30 s的故障發(fā)生時(shí)姿態(tài)角會發(fā)生短暫的波動(dòng)，本文設(shè)計(jì)的觀測器能夠檢測到故障的發(fā)生并將其隔離，之后得到的姿態(tài)角與無故障時(shí)基本一致，從而保證飛行器的穩(wěn)定可靠飛行。

圖11 同時(shí)故障時(shí)姿態(tài)角對比曲線

4 結(jié)語

構(gòu)造快速、準(zhǔn)確的傳感器故障診斷環(huán)節(jié)是提高四旋翼無人飛行器系統(tǒng)的容錯(cuò)能力，保證其在飛行作業(yè)過程中安全飛行的重要基礎(chǔ)。本文針對四旋翼無人飛行器多傳感器故障的診斷問題，設(shè)計(jì)了基于自適應(yīng)觀測器的多傳感器故障診斷方法。對所提出的診斷方法進(jìn)行了理論分析，并通過數(shù)值仿真和實(shí)際飛行驗(yàn)證了所提出方法的有效性。實(shí)驗(yàn)結(jié)果表明所設(shè)計(jì)的故障觀測器可快速地檢測到故障的發(fā)生位置，并實(shí)現(xiàn)故障隔離。在多傳感器發(fā)生故障后，自適應(yīng)故障偏差觀測器能夠準(zhǔn)確估計(jì)各傳感器故障偏差值，有效地提高了飛行器的可靠性和安全性。