基于角加速度估計的非線性增量動態(tài)逆控制及試飛

黨小為，唐鵬，孫洪強，鄭琛

1. 北京航空航天大學 能源與動力工程學院，北京 100083

2. 北京航空航天大學 交通科學與工程學院，北京 100083

非線性動態(tài)逆(Nonlinear Dynamic Inversion， NDI)控制作為一種非線性控制方法，通過對消非線性項來精確線性化系統(tǒng)，進而利用線性控制律設(shè)計方法來設(shè)計控制律，該方法在飛行控制領(lǐng)域中得到了廣泛的應用[1]。但其對系統(tǒng)的精確模型依賴度高導致魯棒性不足。國內(nèi)外有許多學者對動態(tài)逆方法的魯棒性問題進行了研究，一部分研究[2-4]采用神經(jīng)網(wǎng)絡等智能控制方法對模型偏差進行補償，還有部分研究[5-7]將NDI與魯棒控制方法結(jié)合進行設(shè)計。這些方法設(shè)計的控制器結(jié)構(gòu)較為復雜，階次較高，不利于工程實現(xiàn)。20世紀末，Bugajski和Enns[8]，Bacon等[9]提出了一種動態(tài)逆的控制方法，利用增量形式的控制方程，通過狀態(tài)量的速率即角加速度反饋來降低控制律對于被控對象模型的依賴性，提高了魯棒性。

在非線性增量動態(tài)逆(Incremental Nonlinear Dynamic Inversion， INDI)方法的基礎(chǔ)之上，研究者等進行了相關(guān)的研究工作，將其廣泛地應用于飛行控制領(lǐng)域[10-12]。INDI方法依賴角加速度反饋，而一般飛控系統(tǒng)無法直接測量角加速度，針對INDI對于角加速度反饋的需求，有各種角加速度估計方法被提出。常規(guī)的方法是利用角速度差分結(jié)合低通濾波進行角加速度估計，但低通濾波帶來的時間延遲會影響系統(tǒng)的穩(wěn)定性，對此，Koschorke等[13]研究了時間延遲對INDI閉環(huán)系統(tǒng)穩(wěn)定性的影響，Smeur等[14]和周池軍等[15]利用低通濾波估計角加速度的同時，對控制量進行濾波補償。Bacon和Ostroff[16]、陳海兵等[17]利用幾何方法估計角加速度，但此方法數(shù)值求解較為復雜，不利于工程應用。尹航等[18]利用卡爾曼濾波(Kalman Filter，KF)估計角加速度，通過仿真實驗證明了該方法的有效性。

本文針對小型三旋翼無人機(Unmanned Aerial Vehicle，UAV)飛行平臺，在增量動態(tài)逆控制方法的基礎(chǔ)之上，利用飛控慣性測量單元測得的角速度通過卡爾曼濾波角加速度估計器實時提供角加速度反饋，分回路設(shè)計了姿態(tài)控制器，并利用基于模型的控制系統(tǒng)設(shè)計方法將控制器實現(xiàn)。最后，通過試飛試驗驗證基于角加速度估計器的增量動態(tài)逆控制方法的有效性和魯棒性。

1 三旋翼動力學模型

本文所采用的三旋翼無人機模型如圖1所示。其簡化模型的受力分析，如圖2所示。圖中：F1、F2、F3為3個電機產(chǎn)生的拉力，為克服3個電機轉(zhuǎn)動產(chǎn)生的偏航力矩，前2個電機為可傾轉(zhuǎn)差動設(shè)計；δ為前2個電機的差動角；l0為1號、2號電機連線到重心的距離；l1和l2為1號、 2號電機到x軸的距離；l3為3號電機到重心的距離。

機體軸系下的三旋翼姿態(tài)轉(zhuǎn)動動力學方程為

圖1 無人機模型

圖2 受力分析圖

(1)

定義控制輸入量和簡化后三旋翼無人機的轉(zhuǎn)動慣量矩陣分別為

u=[F1,F2,F3,δ]T

式中：p、q、r為無人機繞機體軸轉(zhuǎn)動的角速度。

2 三旋翼控制系統(tǒng)設(shè)計

2.1 增量動態(tài)逆控制律

考察的飛機非線性動力學系統(tǒng)方程為

(2)

式中：x為n維狀態(tài)量；u為p維控制輸入；y為m維輸出。

設(shè)某一時刻的狀態(tài)量和輸入量分別為(x0,u0)，則在(x0,u0)的某鄰域內(nèi)，非線性狀態(tài)方程可利用泰勒一階展開近似為

(3)

(x0,u0)點的狀態(tài)方程為

(4)

定義2個偏導數(shù)為

(5)

式(3)可整理為

(6)

輸出方程對時間求導可得

(7)

Δu=(hxB(x0,u0))-1·

(8)

當控制律的解算頻率足夠高，步長足夠小時，狀態(tài)量的增量Δx相對于控制輸入的增量Δu來說是高階小量，故可略去，則增量動態(tài)逆控制律為

(9)

一般來說，控制輸入的維數(shù)p大于輸出維數(shù)m，故(hxB(x0,u0))-1為廣義右逆。控制律中不顯含非線性狀態(tài)項f(x)，降低了控制律對于飛機本體數(shù)學模型的依賴，使得控制律對于飛機本體數(shù)學模型中的參數(shù)攝動具有一定的魯棒性。同時，采用增量式的控制律算法，使得控制律需要利用到狀態(tài)量的導數(shù)，在三旋翼姿態(tài)控制中意味著需要角加速度反饋量，因此需要利用到第3節(jié)所提到的角加速度估計器。

2.2 控制律設(shè)計

對于本文所研究的被控對象即三旋翼無人機的姿態(tài)控制而言，被控對象的動力學方程如式(1)所示。

定義狀態(tài)量x=Ω=[p,q,r]T，控制輸入為u=[F1,F2,F3,δ]T，輸出為y=x=Ω。故由式(9)可得相應的增量動態(tài)逆控制律為

(10)

根據(jù)三旋翼無人機姿態(tài)控制的特性，基于以上的增量動態(tài)逆控制律，對姿態(tài)控制進行雙回路控制律設(shè)計。

外回路包括姿態(tài)角回路(慢回路)和角速度回路(快回路)，控制框圖如圖3所示。ωφ、ωθ、ωψ和ωp、ωq、ωr分別表示姿態(tài)角回路和角速度回路的帶寬?？旎芈穾捯话銥槁芈穾挼?～5倍，具體設(shè)置為：ωφ=ωθ=ωψ=3 rad/s，ωp=ωq=ωr=10 rad/s；下標c表示指令。

圖3 外回路控制框圖

圖4 內(nèi)回路控制框圖

內(nèi)回路即為角加速度回路，采用增量動態(tài)逆控制律，控制框圖如圖4所示，u0為上一采樣時刻的控制輸入量，因為無法直接反饋實際電機的拉力和舵機偏轉(zhuǎn)的角度，因此加入執(zhí)行機構(gòu)模型模擬電機和舵機的動力學環(huán)節(jié)，同時加入角加速度估計器實時地估計飛機的角加速度。

3 角加速度估計器設(shè)計

3.1 基于卡爾曼濾波器的角加速度估計

INDI是一種對于模型參數(shù)不敏感的魯棒非線性控制方法，該方法利用角加速度反饋進行解算，而一般飛控硬件采用慣性測量單元測量角速度，沒有角加速度傳感器直接測量角加速度。直接利用角速度差分得到的角加速度噪聲過大，無法用于控制律的解算，因此需要利用傳感器的信號估計出角加速度信號。傳統(tǒng)的方式是直接對傳感器的角速度信號進行差分，然后再進行低通濾波。在此基礎(chǔ)之上，本文利用KF進行角加速度的實時估計。

1) 運動學模型

(11)

ΔT為采樣周期;z為觀測量，觀測量為俯仰角速度，來源為慣性測量單元(IMU)的角速率；Q和R分別為過程噪聲和觀測噪聲。

2) 濾波估計過程[19]

角加速度估計的過程即卡爾曼濾波器的遞推求解過程，故可以用于控制律中進行實時的角加速度反饋。

① 一步狀態(tài)更新，根據(jù)上一采樣時刻估計出的狀態(tài)量經(jīng)過狀態(tài)轉(zhuǎn)移矩陣得出一步狀態(tài)量估計值：

(12)

② 狀態(tài)誤差協(xié)方差一步更新，求解出狀態(tài)誤差協(xié)方差陣經(jīng)過一步狀態(tài)更新并引入過程噪聲之后的估計值：

(13)

③ 求解使得狀態(tài)量估計為最小方差無偏估計的卡爾曼增益K：

(14)

④ 根據(jù)卡爾曼增益進行狀態(tài)量和協(xié)方差的校正：

(15)

3) 角加速度估計結(jié)果

根據(jù)三旋翼飛行平臺實際試飛過程中測量的俯仰角速度q，分別利用直接差分、直接差分加低通濾波、卡爾曼濾波器估計3種方法得到相應的角加速度，并進行比較，如圖5所示。

可見，對角速度信號進行直接差分會將噪聲放大，采用傳統(tǒng)的差分加低通濾波則會帶來一定的延遲，而用KF進行角加速度估計可以去除高頻噪聲并保證較小的延遲。

圖5 角加速度估計對比圖

3.2 角加速度估計的時間延遲

互相關(guān)函數(shù)是信號分析里衡量2個信號之間相互關(guān)聯(lián)程度的函數(shù)?？梢酝ㄟ^求取互相關(guān)函數(shù)得到2個相似信號之間的時間延遲。

對于連續(xù)信號定義為

(16)

即將g(t)對時間軸原點進行翻轉(zhuǎn)后與f(t)作卷積。

對于離散時間序列定義為

(17)

它表示序列x(n)不動，將y(n)在時間軸上向左或右移動m個采樣間隔后與序列x(n)相乘后求和，得到平移m之后的對應互相關(guān)函數(shù)值rxy(m)，該值越大表示此時兩信號越相似。因此可以通過互相關(guān)函數(shù)曲線上的峰值點得出2個相似信號之間的時間延遲。

定義歸一化的互相關(guān)函數(shù)，互相關(guān)函數(shù)越接近1表示此時兩信號越相似。

(18)

考察利用卡爾曼濾波得到的角加速度估計值和傳統(tǒng)差分加低通濾波分別帶來的時間延遲。選擇直接進行數(shù)值差分得到的角加速度信號作為基準信號，認為此信號無時間延遲，分別計算2種方法與基準信號之間的互相關(guān)函數(shù)來比較兩者的時間延遲。數(shù)據(jù)來源同3.1節(jié)中數(shù)據(jù)。

由圖6結(jié)果可知，KF方法的時間延遲為0.23個 采樣間隔，而差分加低通濾波方法的時間延遲為0.46個采樣間隔(由于兩者延遲都小于1個 采樣間隔，故求取互相關(guān)函數(shù)之后采用插值的方法得到了平移非整數(shù)采樣間隔點的互相關(guān)函數(shù)值)。故結(jié)果表明，采用KF進行角加速度估計可以去除高頻噪聲并保證較小的延遲。

圖6 時間延遲對比

4 飛行試驗驗證

4.1 控制律實現(xiàn)及試飛平臺

為驗證增量動態(tài)逆控制律和角加速度估計器的可行性，本文進一步基于小型三旋翼無人機平臺開展了飛行試驗，試驗所用三旋翼無人機平臺如圖7所示，其基本參數(shù)如表1所示。

圖7 無人機實物圖

表1 無人機基本參數(shù)

無人機飛控硬件平臺為雷迅pixhack V3，內(nèi)置3組IMU冗余設(shè)計(包含3組加速度計、3組陀螺儀、1組電子羅盤、2組數(shù)字氣壓計)。飛行控制律設(shè)計平臺為MATLAB/SIMULINK，利用基于模型的設(shè)計方法[20]，在SIMILINK中搭建好控制器仿真模型之后，通過SIMULINK代碼自動生成技術(shù)生成可執(zhí)行文件到目標硬件中，并進行試飛測試等等。具體流程如圖8所示。

圖8 控制律設(shè)計流程圖

4.2 試飛結(jié)果

三旋翼無人機姿態(tài)控制試驗在室內(nèi)無風條件下進行。測試姿態(tài)角的跟蹤情況，驗證角加速度估計器和增量動態(tài)逆控制律的有效性。

1) 三軸姿態(tài)響應

圖9為三軸姿態(tài)響應仿真圖。

圖9 三軸姿態(tài)響應

2) 魯棒性測試

在三旋翼無人機平臺上附加重物，改變飛行器的重心位置，測試姿態(tài)角的跟蹤情況，同時與常規(guī)PID控制律進行對比，考察INDI控制律對于飛機本體模型參數(shù)攝動的魯棒性。

重心位置攝動俯仰角跟蹤情況如圖10所示。改變?nèi)砥渲幸粋€動力單元的螺旋槳的尺寸，以達到改變執(zhí)行機構(gòu)效能的目的，考察控制律對于執(zhí)行機構(gòu)效能攝動的魯棒性。

動力效能攝動俯仰角跟蹤情況如圖11所示?？梢悦黠@看出，在重心位置和動力效能有較大攝動時，常規(guī)PID控制律存在很大的穩(wěn)態(tài)誤差，無法很好地跟蹤指令，而基于角加速度反饋的增量動態(tài)逆控制律由于降低了對飛機本體模型的敏感度，故仍能實現(xiàn)很好的指令跟蹤效果，具有較好的魯棒性。

圖10 重心位置攝動測試結(jié)果

圖11 動力效能攝動測試結(jié)果

3) 指令跟蹤

圖12為俯仰角、俯仰角速度以及俯仰角加速度的指令跟蹤情況。圖13為利用角速度差分加低通濾波進行角加速度估計得到的俯仰角指令跟蹤情況。

試飛數(shù)據(jù)顯示，俯仰角、俯仰角速度和俯仰角加速度均能夠很好的跟蹤指令，響應動態(tài)良好。俯仰角響應與俯仰角指令之間的時間延遲約為0.3 s， 與姿態(tài)角回路 3 rad/s 的帶寬一致。穩(wěn)定點附近有小幅的2 °左右的振蕩，該振蕩是由傳感器噪聲以及氣動-結(jié)構(gòu)耦合振動等因素導致。角加速度響應通過角加速度估計器實時估計得到，可以看出，2種角加速度估計方法均能達到控制效果，利用本文的角加速度估計器可以在濾除高頻噪聲的同時保證較小的時間延遲，保障了更好的穩(wěn)定性。

圖12 指令跟蹤結(jié)果

圖13 差分加低通濾波指令跟蹤結(jié)果

5 結(jié) 論

1) INDI控制律可以運用于實際飛行器的控制中，并且對飛機本體模型參數(shù)攝動具有很好的魯棒性。

2) 角加速度估計器為INDI控制律提供必需的角加速度信息，為控制律的實現(xiàn)提供了一種工程上的解決途徑。

3) 基于卡爾曼濾波的角加速度估計方式在濾除高頻噪聲的基礎(chǔ)上帶來的時間延遲更小。