基于SADRC 的四旋翼飛行器姿態(tài)解耦控制方法*

萬 慧，齊曉慧，李 杰，朱子薇，孟麗潔，楊 森

（1.陸軍工程大學(xué)無人機(jī)工程系，石家莊 050003；2.北方自動(dòng)控制技術(shù)研究所，太原 030006）

0 引言

近年來，四旋翼飛行器因其能夠垂直起降，對(duì)起飛著陸場(chǎng)地要求低、定點(diǎn)懸停等優(yōu)勢(shì)成為航空領(lǐng)域的研究熱點(diǎn)之一［1］。其在民用領(lǐng)域，如電力巡檢［2］，搶險(xiǎn)救災(zāi)和軍事領(lǐng)域，如偵查［3］、靶機(jī)等方面都具有廣闊的應(yīng)用前景。目前，對(duì)于四旋翼飛行器的研究主要集中在對(duì)四旋翼飛行器的建模［4］、控制［5-8］、航跡規(guī)劃［9］及感知避障［10］方面，而對(duì)四旋翼飛行器控制系統(tǒng)的設(shè)計(jì)是四旋翼飛行器從理論研究到工程應(yīng)用的關(guān)鍵。

基于線性自抗擾控制（Linear Active Disturbance Rejection Control，LADRC）的四旋翼飛行器姿態(tài)控制方法，在四旋翼飛行器非線性模型的基礎(chǔ)上進(jìn)行的控制律設(shè)計(jì)，表現(xiàn)出了良好的魯棒性，而且該控制方法結(jié)構(gòu)簡(jiǎn)單，具有工程應(yīng)用潛力［11］。然而，在實(shí)驗(yàn)中發(fā)現(xiàn)，當(dāng)LADRC 控制器的初始狀態(tài)誤差較大時(shí)，會(huì)產(chǎn)生“峰值”現(xiàn)象，使控制器超調(diào)增大，控制性能惡化，而且線性擴(kuò)張狀態(tài)觀測(cè)器采用常值增益，當(dāng)增益較大時(shí)，系統(tǒng)控制輸入較大，控制器效率降低。非線性自抗擾（Nonlinear Active Disturbance Rejection Control，NLADRC）由于采用了非線性結(jié)構(gòu)，擴(kuò)張狀態(tài)觀測(cè)器增益可變，對(duì)初始狀態(tài)誤差不敏感，并且控制效率高，但是NLADRC 結(jié)構(gòu)復(fù)雜，參數(shù)整定和穩(wěn)定性分析困難。

文獻(xiàn)［12］在對(duì)NLADRC 和LADRC 各自優(yōu)缺點(diǎn)進(jìn)行分析的基礎(chǔ)上，提出了將二者結(jié)合的線性/非線性自抗擾切換控制（Switch in linear- nonlinear Active Disturbance Rejection Control，SADRC）方法，具有控制效率高、魯棒性強(qiáng)、參數(shù)整定簡(jiǎn)單的特點(diǎn)，受其啟發(fā)，為進(jìn)一步發(fā)揮自抗擾控制在四旋翼飛行器姿態(tài)控制中的優(yōu)勢(shì)，針對(duì)四旋翼飛行器姿態(tài)的非線性模型，本文探討了一種基于SADRC 的四旋翼飛行器姿態(tài)控制方法。首先建立了四旋翼飛行器姿態(tài)的非線性耦合模型，引入SADRC 控制器，對(duì)其結(jié)構(gòu)以及相關(guān)參數(shù)整定方法進(jìn)行了介紹，然后，設(shè)計(jì)了基于SADRC 的四旋翼飛行器姿態(tài)控制器，并針對(duì)引入非線性機(jī)制后，系統(tǒng)穩(wěn)定性分析困難的問題，將控制系統(tǒng)轉(zhuǎn)化為Lurie 系統(tǒng)，采用擴(kuò)展圓判據(jù)法對(duì)其穩(wěn)定性進(jìn)行了分析，最后進(jìn)行了仿真驗(yàn)證。

1 3-DOF 四旋翼姿態(tài)模型

實(shí)驗(yàn)室的四旋翼飛行器半實(shí)物仿真平臺(tái)如圖1所示。該四旋翼無人機(jī)本體運(yùn)動(dòng)原理同“十”字型飛行方式的四旋翼無人機(jī)的運(yùn)動(dòng)原理，通過控制螺旋槳的轉(zhuǎn)速實(shí)現(xiàn)四旋翼無人機(jī)三軸的姿態(tài)角的變化。

圖1 四旋翼半實(shí)物仿真平臺(tái)

該平臺(tái)的姿態(tài)動(dòng)力學(xué)模型為：

其中，φ，θ，ψ 分別為飛行器的俯仰角、滾轉(zhuǎn)角、偏航角；l 為四旋翼無人機(jī)的臂長；Vi（i=f，b，l，r）、分別表示“前、后、左、右”4 個(gè)電機(jī)的電壓；Kf表示電機(jī)電壓與升力間的系數(shù)；KM表示電機(jī)電壓與轉(zhuǎn)矩之間的系數(shù)；Kafi（i=x，y，z）分別表示x，y，z 3 軸的控制阻力系數(shù)。

引入虛擬控制量Ui（i=1，2，3），并將各通道間的動(dòng)態(tài)耦合部分視為系統(tǒng)的內(nèi)部擾動(dòng)，同時(shí)考慮各通道中可能存在的外部擾動(dòng)wi（i=1，2，3），則式（1）可以進(jìn)一步整理為如下形式：

表示作用于各通道的內(nèi)擾與外擾的綜合，即“總擾動(dòng)”，具體可表示為：

由式（2）可以看出，若設(shè)計(jì)的控制器可將各通道的“總擾動(dòng)”進(jìn)行較好的跟蹤和補(bǔ)償，則各通道可變?yōu)榇?lián)積分形式，實(shí)現(xiàn)多耦合系統(tǒng)的解耦控制。

2 姿態(tài)解耦控制方法

2.1 SADRC 算法簡(jiǎn)介

參考鑒于線性控制律在實(shí)際應(yīng)用方面的優(yōu)點(diǎn)，本文采用的SADRC 算法實(shí)際上是在LADRC 框架下，進(jìn)行線性擴(kuò)張狀態(tài)觀測(cè)器（Linear Extended State Observer，LESO）和非線性擴(kuò)張狀態(tài)觀測(cè)器（Nonlinear Extended State Observer，NLESO）之間的切換［13］。其具體結(jié)構(gòu)如下：

其中，式（3）為線性控制律，用于補(bǔ)償殘差，提高控制系統(tǒng)性能，ki（i=1，2，…，n）為控制器增益；式（4）為切換擴(kuò)張狀態(tài)觀測(cè)器（Switch in nonlinear-linear Extended State Observer，SESO），用于跟蹤和補(bǔ)償系統(tǒng)的“總擾動(dòng)”。

式（3）、式（4）中，u 和y 分別對(duì)應(yīng)系統(tǒng)的輸入和輸出；zi（i=1，2，…，n）為系統(tǒng)各狀態(tài)的估計(jì)值；zn+1為對(duì)系統(tǒng)總擾動(dòng)的估計(jì)；b 為系統(tǒng)參數(shù)，設(shè)已知關(guān)于b的部分信息b0，并假定b0≈b；β0i（i=1，2，…，n+1）為SESO 中NLESO 的增益系數(shù)，并假設(shè)SESO 中LESO的增益β0iL（i=1，2，…，n+1）是其NLESO 增益β0i（i=1，2，…，n+1）的（i=1，2，…，n+1）倍，（i=1，2…，n+1）為常數(shù)；則切換函數(shù)fsi（e）（i=1，2，…，n+1）具體可表示為：

LESO 與NLESO 的切換策略具體如下：

1）如果已知系統(tǒng)的初始狀態(tài)誤差，則為避免LADRC 控制器的“峰值”現(xiàn)象，在系統(tǒng)的過渡時(shí)間內(nèi)均采用非線性擴(kuò)張狀態(tài)觀測(cè)器；如果初始狀態(tài)誤差未知，則跳過步驟1），直接轉(zhuǎn)到步驟2）；

2）根據(jù)狀態(tài)誤差e 大小，控制器在LESO 和NLESO 間切換。具體方法為：將線性段區(qū)間長度δs與誤差e 的關(guān)系作為切換策略依據(jù)，預(yù)先設(shè)定δs，（其具體值的確定將在后續(xù)參數(shù)整定中說明），當(dāng)e＜δs時(shí)，采用NLESO 估計(jì)系統(tǒng)的“總擾動(dòng)”；反之，則采用LESO 估計(jì)系統(tǒng)的“總擾動(dòng)”。這里，δs為LESO 與ESO 估計(jì)性能的臨界值，當(dāng)e＜δs時(shí)，ESO 的估計(jì)性能優(yōu)于LESO，反之，則LESO 相比ESO 具有更好的估計(jì)性能。

上述即為SESO 控制器的切換策略，其流程如圖2 所示。

圖2 SESO 切換策略流程圖

2.2 SADRC 參數(shù)整定

由于本文中采用的SADRC 仍采用線性控制律，因此，LADRC 中的線性控制律參數(shù)整定方法仍然適用于式（3），故涉及的參數(shù)整定主要是對(duì)NLESO 增益β0i（i=1，2，…，n+1）的整定和δs的確定。

首先討論NLESO 增益β0i（i=1，2，…，n+1）的整定方法。

令

將式（5）代入式（4）可得：

因此，可從“帶寬法”角度出發(fā)，對(duì)NLESO 的增益β0i（i=1，2，…，n+1）進(jìn)行整定。具體方法在文獻(xiàn)［14］中已經(jīng)給出，這里僅給出整定過程，具體如下：

1）首先對(duì)被控對(duì)象對(duì)應(yīng)的n+1 階LADRC 控制器進(jìn)行整定，得到的觀測(cè)器帶寬記為ωoL；

2）根據(jù)文獻(xiàn)［14］中的參數(shù)整定原則，在通過優(yōu)化算法得到常用參數(shù)優(yōu)化值的基礎(chǔ)上，對(duì)得到結(jié)果進(jìn)行擬合得到參數(shù)整定的經(jīng)驗(yàn)公式。

目前，針對(duì)δs的確定主要有實(shí)驗(yàn)法和理論分析法兩種。實(shí)驗(yàn)法的主要思想是：將SADRC 應(yīng)用于實(shí)際的被控對(duì)象或仿真環(huán)境下對(duì)象的模型，并施加一個(gè)較大的擾動(dòng)，然后給定δs一個(gè)初值，重復(fù)調(diào)整δs直到在該δs下，SADRC 中線性自抗擾控制器和非線性自抗擾控制器性能均達(dá)到最優(yōu)，則此時(shí)的δs即為切換策略的臨界點(diǎn)。

理論分析法的主要思想為當(dāng)系統(tǒng)到達(dá)臨界點(diǎn)時(shí)，LESO 與NLESO 對(duì)系統(tǒng)狀態(tài)及總擾動(dòng)的估計(jì)誤差eiL（i=1，2，…，n+1）、eiNL（i=1，2，…，n+1）的絕對(duì)值應(yīng)相同。因此，分別求出eiL（i=1，2，…，n+1）、eiNL（i=1，2，…，n+1）的表達(dá)式，再使兩式相等，即可求得臨界點(diǎn)δs。

2.3 基于SADRC 的四旋翼姿態(tài)解耦控制方法

2.3.1 SADRC 姿態(tài)控制器設(shè)計(jì)

應(yīng)用SADRC，可對(duì)四旋翼飛行器的3 個(gè)通道分別進(jìn)行姿態(tài)控制，整個(gè)控制系統(tǒng)結(jié)構(gòu)如圖3 所示。

圖3 SADRC 姿態(tài)解耦控制結(jié)構(gòu)圖

圖中，φr，θr，ψr分別表示給定的各通道的期望值。

令x3=f3（·），f3（·）可導(dǎo)，則偏航通道的模型可表達(dá)為：

其中，b=KM/Jz。

因?yàn)樗男砀魍ǖ澜怦詈蠼Y(jié)構(gòu)類似，下面以偏航通道為例，進(jìn)行SADRC 控制器的設(shè)計(jì)。

為便于對(duì)設(shè)計(jì)的控制回路進(jìn)行穩(wěn)定性分析，假設(shè)在對(duì)偏航通道設(shè)計(jì)的切換擴(kuò)張狀態(tài)觀測(cè)器中，LESO 的增益β0iL（i=1，2，3）是其NLESO 的增益β0i（i=1，2，3）的（i=1，2，3）倍，（i=1，2，3）為常數(shù)，則對(duì)偏航通道SESO 可表達(dá)為：

采用同樣的方法，即可實(shí)現(xiàn)對(duì)四旋翼平臺(tái)各通道的姿態(tài)解耦控制姿態(tài)控制。

2.3.2 穩(wěn)定性分析

以偏航通道為例，對(duì)各通道進(jìn)行閉環(huán)控制回路穩(wěn)定性分析。

將式（9）代入式（8），可得：

聯(lián)立式（8）、式（10）、式（11），得到：

因此，可以將式（13）看作為一個(gè)多入單出Lurie系統(tǒng)，該式可進(jìn)一步擴(kuò)張為一個(gè)多入多出lurie 系統(tǒng)：

式（14）可進(jìn)一步改寫為：

可得

進(jìn)而

因?yàn)槠酵ǖ赖拈]環(huán)傳遞函數(shù)G（s）為：

因此，式（19）在原點(diǎn)處包含3 個(gè)特征根，另外兩個(gè)特征根均為負(fù)實(shí)部，則根據(jù)拓展圓判據(jù)［15］，適當(dāng)選擇控制器參數(shù)，可以保證系統(tǒng)絕對(duì)穩(wěn)定。

3 實(shí)驗(yàn)驗(yàn)證

以實(shí)驗(yàn)室現(xiàn)有3-DOF 四旋翼飛行平臺(tái)為對(duì)象，驗(yàn)證算法的有效性。平臺(tái)相關(guān)參數(shù)為：l=0.197 m，Kf=1.188×10-1NV-1，KM=3.60×10-3NmV-1，Jx=Jy=5.5×10-2Nms-2，Jz=1.1×10-1Nms-2。

以偏航通道為例設(shè)置兩個(gè)實(shí)驗(yàn)進(jìn)行驗(yàn)證分析。實(shí)驗(yàn)1 用于驗(yàn)證初始狀態(tài)誤差對(duì)SADRC、NLADRC、LADRC 3 種控制方法下控制性能的影響，實(shí)驗(yàn)2 用于驗(yàn)證3 種控制方法的抗外擾性能。實(shí)驗(yàn)中SADRC 的相關(guān)參數(shù)如表1 所示。

表1 基于SADRC 的姿態(tài)控制方法參數(shù)

實(shí)驗(yàn)1：初始狀態(tài)誤差對(duì)系統(tǒng)控制性能影響的實(shí)驗(yàn)

圖4 偏航通道方波響應(yīng)曲線（z1=2）

由于該平臺(tái)數(shù)字信號(hào)和模擬信號(hào)的轉(zhuǎn)換是通過數(shù)據(jù)采集卡實(shí)現(xiàn)的，故傳感器得到的數(shù)據(jù)在小范圍內(nèi)有一定的波動(dòng)，圖像不是平滑的。

由圖4 可以看出，在t=0 s 時(shí)，初始狀態(tài)誤差較大，基于LADRC 的控制器由于采用常值增益，產(chǎn)生了類似高增益觀測(cè)器的“峰值”現(xiàn)象，響應(yīng)曲線出現(xiàn)了較大超調(diào)，過渡時(shí)間增加，控制性能降低，而基于SADRC 和NLADRC 控制器的控制性能基本未受影響；而隨著時(shí)間推移，狀態(tài)誤差逐漸減小至0，此時(shí)，3 種控制方法均具有較好的控制效果，但基于SADRC 和NLADRC 控制器的響應(yīng)曲線稍平滑一些，過渡時(shí)間較短。

實(shí)驗(yàn)2：抗外擾性能實(shí)驗(yàn)

圖5 偏航通道階躍響應(yīng)曲線（w=0.5 V）

初始設(shè)置不變，當(dāng)t=3.5 s 時(shí)，通過matlab 預(yù)先設(shè)置在控制電壓Vf中加入w=3 V 的無周期脈沖干擾信號(hào)，重復(fù)上述實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如圖6 所示。

圖6 偏航通道階躍響應(yīng)曲線（w=3 V）

由圖5 和圖6 可以看出，當(dāng)外擾較小時(shí)基于NLADRC 控制器的響應(yīng)曲線更加平滑，超調(diào)較小且過渡時(shí)間較短，控制性能好于LADRC，當(dāng)外擾較大時(shí)則相反；而基于SADRC 的控制器控制性能受外擾大小的影響較小，在兩種外擾條件下均表現(xiàn)出較強(qiáng)的抗干擾性。因?yàn)閷?shí)際工況中所受外擾的大小也是不確定的，因此，引入SADRC，根據(jù)環(huán)境條件進(jìn)行切換控制，以充分發(fā)揮LADRC 和NLADRC 各自的優(yōu)勢(shì)，具有實(shí)際意義。

4 結(jié)論

本文給出了基于SADRC 的四旋翼無人姿態(tài)解耦控制策略，介紹了該控制器中關(guān)鍵參數(shù)的選取方法，將系統(tǒng)轉(zhuǎn)換為多入多出lurie 系統(tǒng)，根據(jù)擴(kuò)展圓判據(jù)對(duì)閉環(huán)控制系統(tǒng)的穩(wěn)定性進(jìn)行了分析，并以半實(shí)物仿真平臺(tái)為對(duì)象，對(duì)算法的控制性能進(jìn)行了驗(yàn)證。實(shí)驗(yàn)結(jié)果表明，基于SADRC 的四旋翼姿態(tài)解耦控制算法，有效避免LADRC 由于“峰值”現(xiàn)象帶來的超調(diào)增大、調(diào)節(jié)時(shí)間增加等問題，且SADRC 對(duì)于大小不同的擾動(dòng)均表現(xiàn)出了具有良好的抗擾性能，響應(yīng)曲線較為平滑且過渡時(shí)間較短，相對(duì)LADRC，控制器性能得到了進(jìn)一步提高。