黨小為，唐鵬，孫洪強(qiáng)，鄭琛

1. 北京航空航天大學(xué) 能源與動(dòng)力工程學(xué)院，北京 100083

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

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

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

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

1 三旋翼動(dòng)力學(xué)模型

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

機(jī)體軸系下的三旋翼姿態(tài)轉(zhuǎn)動(dòng)動(dòng)力學(xué)方程為

圖1 無(wú)人機(jī)模型

圖2 受力分析圖

(1)

定義控制輸入量和簡(jiǎn)化后三旋翼無(wú)人機(jī)的轉(zhuǎn)動(dòng)慣量矩陣分別為

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

式中：p、q、r為無(wú)人機(jī)繞機(jī)體軸轉(zhuǎn)動(dòng)的角速度。

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

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

考察的飛機(jī)非線性動(dòng)力學(xué)系統(tǒng)方程為

(2)

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

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

(3)

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

(4)

定義2個(gè)偏導(dǎo)數(shù)為

(5)

式(3)可整理為

(6)

輸出方程對(duì)時(shí)間求導(dǎo)可得

(7)

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

(8)

當(dāng)控制律的解算頻率足夠高，步長(zhǎng)足夠小時(shí)，狀態(tài)量的增量Δx相對(duì)于控制輸入的增量Δu來(lái)說(shuō)是高階小量，故可略去，則增量動(dòng)態(tài)逆控制律為

(9)

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

2.2 控制律設(shè)計(jì)

對(duì)于本文所研究的被控對(duì)象即三旋翼無(wú)人機(jī)的姿態(tài)控制而言，被控對(duì)象的動(dòng)力學(xué)方程如式(1)所示。

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

(10)

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

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

圖3 外回路控制框圖

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

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

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

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

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

1) 運(yùn)動(dòng)學(xué)模型

(11)

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

2) 濾波估計(jì)過(guò)程[19]

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

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

(12)

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

(13)

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

(14)

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

(15)

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

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

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

圖5 角加速度估計(jì)對(duì)比圖

3.2 角加速度估計(jì)的時(shí)間延遲

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

對(duì)于連續(xù)信號(hào)定義為

(16)

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

對(duì)于離散時(shí)間序列定義為

(17)

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

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

(18)

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

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

圖6 時(shí)間延遲對(duì)比

4 飛行試驗(yàn)驗(yàn)證

4.1 控制律實(shí)現(xiàn)及試飛平臺(tái)

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

圖7 無(wú)人機(jī)實(shí)物圖

表1 無(wú)人機(jī)基本參數(shù)

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

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

4.2 試飛結(jié)果

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

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

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

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

2) 魯棒性測(cè)試

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

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

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

圖10 重心位置攝動(dòng)測(cè)試結(jié)果

圖11 動(dòng)力效能攝動(dòng)測(cè)試結(jié)果

3) 指令跟蹤

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

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

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

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

5 結(jié) 論

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

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

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