無人機閉環(huán)控制系統(tǒng)局部飛行狀態(tài)估計

曹立佳,劉明濤,李杰夫

(1.人工智能四川省重點實驗室， 四川 自貢 643000; 2.四川輕化工大學(xué) 自動化與信息學(xué)院， 四川 自貢 643000; 3.四川省智慧旅游研究基地， 四川 自貢 643000)

無人機由于具有低風(fēng)險、低成本、可自主飛行和能重復(fù)利用的特點，正在成為一種越來越受重視的新興航空飛行器，開始被廣泛應(yīng)用于軍事、民用和科學(xué)研究領(lǐng)域。相比傳統(tǒng)有人駕駛飛機，無人機多數(shù)體積較小，成本低，所使用的傳感器設(shè)備往往精度和可靠性較低。特別是小型無人機基本都采用微加速度計和微陀螺儀等微機電系統(tǒng)(micro-electro mechanical system,MEMS)作為慣性測量組合(inertial measurement unit,IMU)元件。MEMS器件雖然價格低廉，但精度相對較差，存在隨機游走與大量測量噪聲是不可避免的問題，且無人機上的振動等也會對慣性測量器件造成較大的測量誤差[1]。姿態(tài)控制是無人機飛行控制的基礎(chǔ)與核心，其控制性能的好壞對無人機飛行的安全與效能有極大地影響[2].由于無人機系統(tǒng)作為一個典型非線性系統(tǒng)，具有強非線性，強耦合性和時變性。當(dāng)前無人機的飛行控制研究主要集中非線性控制方法上[3]，比如動態(tài)逆[4]、反步法[5]、模糊控制[6]、滑模控制[7]等等。然而，這些先進的控制方法將更多的焦點集中于系統(tǒng)存在不確定性或擾動的問題上，基本都基于系統(tǒng)狀態(tài)是能夠且精確得到的。但實際情況是，由于傳感器設(shè)置及其測量本身的原因，無人機系統(tǒng)的狀態(tài)向量往往無法全部或精確得到。也有許多研究人員進行了存在模型不確定性和外界擾動情況下的自適應(yīng)控制方法的研究[8]，但這些方法多將模型不確定性和外界擾動看作未知項進行自適應(yīng)近似，對于系統(tǒng)采用低成本傳感器的適用性還有待進一步探討。無人機飛行姿態(tài)估計也有一些專門的研究。如：賀思捷等[9]采用遙測數(shù)據(jù)驅(qū)動的方法對無人機飛行狀態(tài)進行識別，這種方法適用于外部對無人機的飛行狀態(tài)進行判斷，無法用于其自身的飛行控制；Di[10]、續(xù)立軍等[11]通過引入紅外地平儀實現(xiàn)對無人機姿態(tài)進行解算，但這類方法需要增加新的紅外地平設(shè)備。

實質(zhì)上，對小型無人機飛行狀態(tài)估計問題可以看作是含有噪聲的動態(tài)系統(tǒng)狀態(tài)估計問題。而卡爾曼濾波器是解決這一問題最有效的方法之一。擴展卡爾曼濾波器(extended kalman filter，EKF)具有良好的工作性能以及適當(dāng)?shù)挠嬎阖?fù)擔(dān)，特別適用于小型無人機這樣的非線性系統(tǒng)，目前EKF在飛行器的組合導(dǎo)航、狀態(tài)估計中已得到了廣泛的應(yīng)用。如Jaime等[12]采用EKF對隨機風(fēng)場中固定翼無人機的狀態(tài)進行估計；Zhang等[13]將EKF用于多旋翼無人機的姿態(tài)估計。在已有的研究成果中，針對無人機開環(huán)模型進行設(shè)計的居多，考慮飛行控制系統(tǒng)閉環(huán)結(jié)構(gòu)下估計方法的研究相對較少。國內(nèi)北航的鐘麥英團隊進行了相關(guān)的研究，如：劉曉東等[14]采用EKF對實現(xiàn)了風(fēng)擾影響下無人機閉環(huán)控制系統(tǒng)的故障檢測，但只考慮了無人機的縱向運動模型，控制器采用傳統(tǒng)PID控制，噪聲強度也很小。而對于全狀態(tài)運動模型，往往狀態(tài)估計器維數(shù)高，難以在微型處理器上實現(xiàn)。

本文主要考慮強噪聲條件下的小型無人機控制閉環(huán)飛行狀態(tài)估計問題。以某無人機六自由度非線性閉環(huán)系統(tǒng)模型為基礎(chǔ)，選取易對受傳感器測量誤差影響的氣流角和機體旋轉(zhuǎn)角速率作為關(guān)鍵系統(tǒng)狀態(tài)，設(shè)計基于擴EKF的狀態(tài)估計器對其進行控制閉環(huán)系統(tǒng)的局部狀態(tài)估計，并以此作為飛行控制器的狀態(tài)反饋輸入，實現(xiàn)強噪聲條件下的小型無人機的穩(wěn)定控制。

1 系統(tǒng)模型與問題描述

本研究對象為中低速短時飛行的小型無人機系統(tǒng)。通常可以假定地球為平坦面，無人機質(zhì)量變化不大且具有面對稱外形。這樣，無人機系統(tǒng)的動力學(xué)方程在混合坐標(biāo)系下(包含機體坐標(biāo)系和風(fēng)速坐標(biāo)系)[15,16]可以描述為如下方程組：

(1)

其中:Ix、Iy和Iz分別為三個軸向的轉(zhuǎn)動慣量；Ixz為慣性積。

空氣動力Xw、Yw、Zw和動力矩L、M、N一般可以用多項式來近似表達[17]。

無人機六自由度非線性系統(tǒng)(1)中，選取系統(tǒng)狀態(tài)變量x∈R9和控制輸入u∈R3如式(2)所示：

(2)

則所討論的無人機系統(tǒng)的非線性系統(tǒng)模型及觀測模型可分別記作式(3)和式(4)：

(3)

y=h(x,t)

(4)

式中，y為系統(tǒng)觀測值；f(x,t)、g(x,t)和h(x,t)均為非線性向量函數(shù)或矩陣。

如前所述，對于式(3)和式(4)這樣的非線性系統(tǒng)，當(dāng)前有不少效果很好的非線性控制方法去實現(xiàn)對其的穩(wěn)定控制。但需要注意的是，對于小型無人機系統(tǒng)而言，系統(tǒng)方程和量測方程中分別存在不可避免的系統(tǒng)噪聲w(t)與量測噪聲v(t)。則含有噪聲和無人機系統(tǒng)模型可改寫為式(5)和式(6)：

(5)

y=h(x,t)+v(t)

(6)

對于式(5)和式(6)，設(shè)計穩(wěn)定控制器前提是如何得到系統(tǒng)狀態(tài)精確估計值。

2 基于EKF的局部飛行狀態(tài)估計器

由于小型無人機系統(tǒng)主要飛行狀態(tài)，如攻角，側(cè)滑角和機體旋轉(zhuǎn)角速率等信號對于飛行姿態(tài)控制來說往往是非常關(guān)鍵的，且受傳感器噪聲影響明顯。因此，可選取無人機六自由度非線性系統(tǒng)(1)中受傳感器噪聲影響嚴(yán)重的部分系統(tǒng)狀態(tài)變量xF∈R5，量測輸出yF∈R5和控制輸入u∈R3，進行重點分析。如式(7)所示：

(7)

無人機系統(tǒng)方程具有明顯的非線性特性，經(jīng)典卡爾曼濾波器難以適用，因此，采用基于非線性動態(tài)和/或觀測模型在狀態(tài)估計時刻的實時線性泰勒展開近似的EKF[18]進行狀態(tài)估計可以在估計準(zhǔn)確性和計算量之間找到一個平衡。

根據(jù)無人機非線性連續(xù)系統(tǒng)模型(5)和(6)，可以推導(dǎo)出其帶有噪聲的線性離散微分方程式(8)和(9)：

(8)

(9)

(10)

k時刻的轉(zhuǎn)移矩陣Φk可以通過式(11)進行一階近似。

Φk≈I+FkTs

(11)

(12)

根據(jù)無人機非線性動態(tài)模型，式(12)中連續(xù)系統(tǒng)動態(tài)矩陣Fk的元素如下：

同時，系統(tǒng)(8)中的離散控制輸入矩陣為：

Gk=Gc(k)·Ts

(13)

式中，Gc(k)為連續(xù)控制輸入矩陣，可以通過式(14)來計算。

(14)

MCA通常被稱為控制效率矩陣，主要反映無人機控制舵面的效率。有：

(15)

(16)

則EKF設(shè)計過程中的預(yù)測方程和更新方程可以由式(17)～(22)給出。

(17)

狀態(tài)預(yù)測方程：

(18)

方差矩陣預(yù)測方程：

(19)

Kalman增益矩陣方程：

(20)

方差矩陣估計方程：

Pk+1/k+1=[I-Kk+1Hk]Pk+1/k

(21)

狀態(tài)估計方程：

(21)

(23)

(24)

3 仿真試驗與分析

為了驗證本文方法的有效性，以某小型無人機系統(tǒng)模型為基礎(chǔ)，根據(jù)前面提出的設(shè)計方法，進行了數(shù)字仿真試驗。仿真模型在Matlab/Simulink環(huán)境中搭建，飛行控制器和狀態(tài)濾波器采用Embedded Matlab函數(shù)實現(xiàn)。整個系統(tǒng)的仿真試驗在Links-Box實時仿真系統(tǒng)上實現(xiàn)。

根據(jù)無人機狀態(tài)測量實際，在狀態(tài)變量中加入高斯白噪聲來模擬無人機所使用的典型低成本傳感器測量得到的含有噪聲的狀態(tài)測量信號。傳感器典型噪聲特性[21]：角速率信號的噪聲分布為標(biāo)準(zhǔn)差σp,q,r=5 deg/s=0.087 3 rad/s，噪聲方差∑p,q,r=0.007 6×I3rad2/s2，氣流角信號的噪聲分布為標(biāo)準(zhǔn)差σα,β=2 deg/s=0.034 9 rad/s，噪聲方差∑α，β=0.001 2×I2rad2/s2。

EKF的過程噪聲方差矩陣和量測噪聲方差矩陣分別設(shè)為，。

為驗證采用EKF后閉環(huán)控制系統(tǒng)狀態(tài)估計和控制效果的性能，在仿真試驗中進行兩組仿真試驗作為對比：一組將EKF包含在控制閉環(huán)系統(tǒng)中(即采用EKF估計值作為控制器的狀態(tài)信息)，另一組則不將EKF包含在閉環(huán)控制系統(tǒng)中(即EKF估計值僅作為狀態(tài)監(jiān)測，不作為控制器設(shè)計的狀態(tài)信息)。兩組仿真試驗其余條件完全相同。所采用的控制器參數(shù)分別為：k1=6，k2=4。

仿真結(jié)果如圖 1、圖2所示。從圖1可以看出，對于類似于階躍信號的這種突變指令，系統(tǒng)控制器也能很好跟蹤。EKF不在閉環(huán)的控制效果明顯比EKF在閉環(huán)的控制效果差，出現(xiàn)了明顯的波動，特別是圖1(a)中滾轉(zhuǎn)角指令最大誤差達到1.8°，最大偏差高達45%。圖1(a)含有EKF狀態(tài)估計的閉環(huán)控制系統(tǒng)對于滾轉(zhuǎn)角指令的跟蹤穩(wěn)定時間小于1 s，響應(yīng)超調(diào)為2.41%，穩(wěn)態(tài)誤差不大于0.80%。圖1(b)俯仰角指令信號跟蹤的響應(yīng)時間小于1 s，穩(wěn)態(tài)誤差不大于1.16%。

圖1 角度指令跟蹤響應(yīng)(Command代表指令信號，虛線EKF out代表EKF不在閉環(huán)系統(tǒng)的情況，實線EKF in代表EKF在閉環(huán)系統(tǒng)中的情況)

圖2 EKF狀態(tài)估計結(jié)果(點線Measurements代表含有噪聲的系統(tǒng)狀態(tài)，虛線EKF out代表EKF不在閉環(huán)系統(tǒng)的情況，實線EKF in代表EKF在閉環(huán)系統(tǒng)中的情況)

圖2(a)-圖2(e)給出了在噪聲情況下兩種狀態(tài)中EKF對系統(tǒng)狀態(tài)估計結(jié)果。從圖中可以看出，模擬的無人機狀態(tài)測量信號(點線Measurements)由于低成本傳感器含有很強的噪聲，使得真實的系統(tǒng)狀態(tài)被噪聲淹沒，無法得到其真實值。但采用EKF進行狀態(tài)估計后對噪聲的消除效果明顯，且在信號突變時刻，如第5 s、30 s時滾轉(zhuǎn)角速率突變，第10 s時俯仰角速率突變，也能夠快速估計出真實值。同時，可以看到EKF不在閉環(huán)系統(tǒng)情況下的估計值(虛線EKF out)比EKF在閉環(huán)系統(tǒng)中的估計值(實線EKF in)有明顯的抖動，這主要是因為EKF不在閉環(huán)系統(tǒng)中時，控制器設(shè)計所采用的系統(tǒng)狀態(tài)是含有噪聲的系統(tǒng)狀態(tài)值，而不是EKF估計值。因此，控制器輸出的控制指令會極大的受到噪聲信息的影響，出現(xiàn)快速變化，控制信號的變化反過來又影響系統(tǒng)狀態(tài)的估計。EKF在閉環(huán)的情況下，控制器采用的EKF的估計值，幾乎不受噪聲的影響，輸出要平滑得多。

仿真試驗的結(jié)果綜合來看，采用EKF濾波器對無人機閉環(huán)系統(tǒng)狀態(tài)進行估計后，可以有消除傳感器噪聲的影響，實現(xiàn)快速有效地跟蹤突變指令，響應(yīng)時間小于1s，跟蹤誤差不大于1.16%。

4 結(jié)論

1) 無人機系統(tǒng)狀態(tài)中氣流角和機體旋轉(zhuǎn)角速率易受傳感器測量噪聲影響，可以將其作為閉環(huán)系統(tǒng)EKF設(shè)計的關(guān)鍵系統(tǒng)狀態(tài)。這樣在降低濾波器維數(shù)時也能保證閉環(huán)系統(tǒng)狀態(tài)估計效果較為理想。

2) 在傳感器噪聲強度較大時，采用反步控制方法仍能取得穩(wěn)定的閉環(huán)控制，但效果欠佳。引入EKF對閉環(huán)系統(tǒng)狀態(tài)進行有效估計后，控制效果提升明顯。

3) 采用EKF對閉環(huán)系統(tǒng)的狀態(tài)進行有效估計后，系統(tǒng)跟蹤控制指令更為平滑，且能夠提高控制指令執(zhí)行效率，對提高系統(tǒng)壽命與降低能耗有益。