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

2020-06-08 01:37:58黨小為唐鵬孫洪強鄭琛

航空學報 2020年4期

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

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

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

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

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

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

1 三旋翼動力學模型

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

機體軸系下的三旋翼姿態轉動動力學方程為

圖1 無人機模型

圖2 受力分析圖

(1)

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

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

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

2 三旋翼控制系統設計

2.1 增量動態逆控制律

考察的飛機非線性動力學系統方程為

(2)

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

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

(3)

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

(4)

定義2個偏導數為

(5)

式(3)可整理為

(6)

輸出方程對時間求導可得

(7)

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

(8)

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

(9)

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

2.2 控制律設計

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

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

(10)

根據三旋翼無人機姿態控制的特性，基于以上的增量動態逆控制律，對姿態控制進行雙回路控制律設計。

外回路包括姿態角回路(慢回路)和角速度回路(快回路)，控制框圖如圖3所示。ωφ、ωθ、ωψ和ωp、ωq、ωr分別表示姿態角回路和角速度回路的帶寬。快回路帶寬一般為慢回路帶寬的3～5倍，具體設置為：ωφ=ωθ=ωψ=3 rad/s，ωp=ωq=ωr=10 rad/s；下標c表示指令。

圖3 外回路控制框圖

圖4 內回路控制框圖

內回路即為角加速度回路，采用增量動態逆控制律，控制框圖如圖4所示，u0為上一采樣時刻的控制輸入量，因為無法直接反饋實際電機的拉力和舵機偏轉的角度，因此加入執行機構模型模擬電機和舵機的動力學環節，同時加入角加速度估計器實時地估計飛機的角加速度。

3 角加速度估計器設計

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

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

1) 運動學模型

(11)

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

2) 濾波估計過程[19]

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

① 一步狀態更新，根據上一采樣時刻估計出的狀態量經過狀態轉移矩陣得出一步狀態量估計值：

(12)

② 狀態誤差協方差一步更新，求解出狀態誤差協方差陣經過一步狀態更新并引入過程噪聲之后的估計值：

(13)

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

(14)

④ 根據卡爾曼增益進行狀態量和協方差的校正：

(15)

3) 角加速度估計結果

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

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

圖5 角加速度估計對比圖

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

互相關函數是信號分析里衡量2個信號之間相互關聯程度的函數。可以通過求取互相關函數得到2個相似信號之間的時間延遲。

對于連續信號定義為

(16)

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

對于離散時間序列定義為

(17)

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

定義歸一化的互相關函數，互相關函數越接近1表示此時兩信號越相似。

(18)

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

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

圖6 時間延遲對比

4 飛行試驗驗證

4.1 控制律實現及試飛平臺

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

圖7 無人機實物圖

表1 無人機基本參數

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

圖8 控制律設計流程圖

4.2 試飛結果

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

1) 三軸姿態響應

圖9為三軸姿態響應仿真圖。

圖9 三軸姿態響應

2) 魯棒性測試

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

重心位置攝動俯仰角跟蹤情況如圖10所示。改變三旋翼其中一個動力單元的螺旋槳的尺寸，以達到改變執行機構效能的目的，考察控制律對于執行機構效能攝動的魯棒性。

動力效能攝動俯仰角跟蹤情況如圖11所示。可以明顯看出，在重心位置和動力效能有較大攝動時，常規PID控制律存在很大的穩態誤差，無法很好地跟蹤指令，而基于角加速度反饋的增量動態逆控制律由于降低了對飛機本體模型的敏感度，故仍能實現很好的指令跟蹤效果，具有較好的魯棒性。

圖10 重心位置攝動測試結果

圖11 動力效能攝動測試結果

3) 指令跟蹤

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

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