一種多旋翼無(wú)人機(jī)自抗擾姿態(tài)控制器

任開(kāi)眾，徐 勝，蘇成悅 ，陳元電

（1.廣東工業(yè)大學(xué)信息工程學(xué)院，廣州 510006；2.廣東工業(yè)大學(xué)物理與光電工程學(xué)院，廣州 510006）

0 引言

近年來(lái)，多旋翼無(wú)人機(jī)憑借可以垂直起降、懸停和易于控制等優(yōu)點(diǎn)，被廣泛用于航拍、測(cè)繪、農(nóng)業(yè)植保、物流運(yùn)輸和消防等多個(gè)領(lǐng)域。隨著無(wú)人機(jī)的應(yīng)用場(chǎng)景越來(lái)越廣泛，對(duì)無(wú)人機(jī)控制的穩(wěn)定性提出了更高的要求。多旋翼無(wú)人機(jī)屬于欠驅(qū)動(dòng)、非線性和強(qiáng)耦合系統(tǒng)，飛行過(guò)程中容易受到風(fēng)力干擾，這使得無(wú)人機(jī)姿態(tài)控制具有一定的挑戰(zhàn)。傳統(tǒng)的無(wú)人機(jī)姿態(tài)控制器是雙環(huán)串級(jí)PID，其原理簡(jiǎn)單，易于實(shí)現(xiàn)，但是存在控制精度不足、調(diào)參復(fù)雜和魯棒性差的缺點(diǎn)，因此不同的控制策略被提了出來(lái)。

文獻(xiàn)［5-8］分別提出了使用非線性PID 和模糊PID 來(lái)提高無(wú)人機(jī)姿態(tài)控制的魯棒性，但仍然存在控制精度不足的缺點(diǎn)。文獻(xiàn)［9］提出了基于神經(jīng)網(wǎng)絡(luò)的自適應(yīng)滑模控制方法，計(jì)算量較大，對(duì)處理器要求高，不易于實(shí)現(xiàn)。文獻(xiàn)［10-13］均采用了自適應(yīng)滑?？刂撇呗?，但是容易產(chǎn)生抖振現(xiàn)象。

自抗擾控制（active disturbance rejection control，ADRC）于1990年由韓京清研究員提出，傳統(tǒng)的ADRC 由二階跟蹤微分器（tracking differentiator，TD）、非線性PID 控制律（NLPID）和擴(kuò)張狀態(tài)觀測(cè)器（extended state observer，ESO）組成。在ADRC 中，外擾和內(nèi)擾被歸為“總擾動(dòng)”。擴(kuò)張狀態(tài)觀測(cè)器是ADRC 的核心部分，在ESO 中，被控對(duì)象模型被等效成串聯(lián)積分系統(tǒng)，總擾動(dòng)被擴(kuò)張成系統(tǒng)的一個(gè)狀態(tài)，不需要精確模型也能很好地估計(jì)出擾動(dòng)來(lái)并補(bǔ)償?shù)?，?duì)噪聲抑制作用明顯，對(duì)多旋翼控制尤為適用。

文獻(xiàn)［16-19］都將自抗擾控制應(yīng)用于四旋翼姿態(tài)控制，與PID 控制器相比，ADRC 控制器在解耦和抗干擾方面都具有良好的性能。

如圖1 所示，本文主要的創(chuàng)新點(diǎn)是在傳統(tǒng)ADRC的基礎(chǔ)上引入了一個(gè)近似模型，無(wú)人機(jī)橫滾和俯仰軸旋轉(zhuǎn)建模分別引入一階慣性模型，并在此模型基礎(chǔ)上根據(jù)李亞普諾夫穩(wěn)定性定理設(shè)計(jì)了反步控制律來(lái)代替?zhèn)鹘y(tǒng)的非線性PID 控制律。為了得到更加平滑的輸入信號(hào)的一階、二階和三階導(dǎo)數(shù)信號(hào)，本文采用四階線性跟蹤微分器代替?zhèn)鹘y(tǒng)ADRC 的最速二階跟蹤微分器。擴(kuò)張狀態(tài)觀測(cè)器同樣引入了一階慣性模型來(lái)預(yù)測(cè)被控對(duì)象的輸出，相比傳統(tǒng)ADRC，總擾動(dòng)可以估計(jì)得更快更準(zhǔn)。同時(shí)設(shè)計(jì)了針對(duì)參數(shù)的無(wú)人機(jī)姿態(tài)自適應(yīng)調(diào)參策略，經(jīng)過(guò)仿真和實(shí)際飛行驗(yàn)證,此姿態(tài)控制器能保證無(wú)人機(jī)在初始參數(shù)與實(shí)際相差10 倍都不會(huì)發(fā)散，極大提升了控制器的魯棒性、調(diào)參簡(jiǎn)易性、飛行的穩(wěn)定性和安全性。

圖1 自抗擾姿態(tài)控制器

1 姿態(tài)控制建模

以X型四旋翼無(wú)人機(jī)為例子,如圖2所示。

圖2 四旋翼姿態(tài)建模示意圖

假設(shè)無(wú)人機(jī)整體為一個(gè)剛體, 質(zhì)量分布均勻,其姿態(tài)動(dòng)力學(xué)方程可表示為：

其中:、和分別是四旋翼無(wú)人機(jī)的橫滾角、俯仰角和偏航角。l、l和l是三軸轉(zhuǎn)動(dòng)慣量。τ、τ和τ為外力擾動(dòng)。l為電機(jī)轉(zhuǎn)動(dòng)慣量。Ω =-+-。是力臂長(zhǎng)度。F、F和F則是電機(jī)輸出升力。

在實(shí)際應(yīng)用中,對(duì)于橫滾俯仰旋轉(zhuǎn)運(yùn)動(dòng)，其力矩須由螺旋槳撥動(dòng)空氣來(lái)產(chǎn)生，存在一定的延時(shí)，而偏航旋轉(zhuǎn)運(yùn)動(dòng)則依靠不同電機(jī)和螺旋槳自身轉(zhuǎn)速差來(lái)達(dá)到力矩輸出，故偏航旋轉(zhuǎn)模型可等效于·，俯仰和橫滾控制量到螺旋槳產(chǎn)生升力的過(guò)程則等效為一階慣性模型，假設(shè)電機(jī)動(dòng)力參數(shù)一致，有：

式中：可認(rèn)為是螺旋槳加速的慣性時(shí)間，為增益，把式（1）中除F外的項(xiàng)當(dāng)成未建模擾動(dòng)，結(jié)合式（3）可得：

2 模型預(yù)測(cè)狀態(tài)觀測(cè)器

以X 軸為例, 選取角速度為觀測(cè)量, 根據(jù)式（4）其旋轉(zhuǎn)動(dòng)力學(xué)狀態(tài)空間方程如下所示：

其中：、和分別是角速度、角加速度和擾動(dòng)，x則是模型預(yù)測(cè)角加速度。模型預(yù)測(cè)擴(kuò)張狀態(tài)觀測(cè)器設(shè)計(jì)如下：

其中：、和分別是估計(jì)角速度、估計(jì)角加速度和估計(jì)擾動(dòng)，z是預(yù)測(cè)角加速度，為無(wú)人機(jī)實(shí)際控制角度，和為觀測(cè)器系數(shù)。

3 四階線性跟蹤微分器

控制器帶寬有限，用來(lái)跟蹤無(wú)限帶寬的輸入信號(hào)本身不合理，并且直接對(duì)帶噪信號(hào)求導(dǎo)會(huì)放大噪聲而導(dǎo)致控制發(fā)散，因此跟蹤微分器是經(jīng)典自抗擾控制中很重要的一部分。跟蹤微分器旨在獲得平滑的輸入信號(hào)及輸入信號(hào)的各階導(dǎo)數(shù)。如果能夠合理地提取差分信號(hào)，可以提高控制器的性能，大大簡(jiǎn)化控制器的設(shè)計(jì)。傳統(tǒng)自抗擾控制采用的是二階最速微分跟蹤器，具有復(fù)雜的函數(shù)形式， 這對(duì)實(shí)際實(shí)現(xiàn)應(yīng)用是一個(gè)挑戰(zhàn)。此外它僅提供一階導(dǎo)數(shù)信號(hào)。四階線性跟蹤微分器可以獲得更高階的導(dǎo)數(shù)信號(hào)用于控制律，其參數(shù)較少，易于實(shí)現(xiàn)，傳遞函數(shù)如式（7）所示：

其中：為跟蹤/濾波因子，＞0。假設(shè)v() =,根據(jù)終值定理有：

式（8）證明此跟蹤微分器的輸出最終會(huì)無(wú)誤差地收斂到輸入值,如圖3所示,其跟蹤速度由參數(shù)決定，參數(shù)越大,跟蹤越快,但是濾波性能會(huì)下降。

圖3 四階線性跟蹤微分器的階躍響應(yīng)

此四階線性跟蹤微分器的離散形式如下所示：

式中：是輸入信號(hào)，、、、分別是跟蹤信號(hào)、跟蹤信號(hào)的一階、二階和三階導(dǎo)數(shù)。

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

選取角度為控制量,系統(tǒng)的狀態(tài)空間方程如式（10）所示：

其中:、和分別是角度、角速度和角加速度, ξ是系統(tǒng)的總擾動(dòng),包含外擾和內(nèi)擾?？刂破髟O(shè)計(jì)如下：

（1）假設(shè)目標(biāo)角度為,角度誤差為,有：

定義Lyapunov 函數(shù)為：

有：

定義Lyapunov函數(shù)為：

有：

定義：

定義Lyapunov函數(shù)為：

有：

即：

式（10）結(jié)合式（23）可得：

最后得到控制量輸出u為：

（4）反推

式（26）—式（28）即是反步姿態(tài)控制器所需的計(jì)算公式。及其各階導(dǎo)數(shù)由四階線性跟蹤微分器給出。

5 自適應(yīng)調(diào)參策略

為解決無(wú)人機(jī)因控制參數(shù)與實(shí)際參數(shù)相差過(guò)大而導(dǎo)致控制發(fā)散的問(wèn)題,針對(duì)參數(shù)提出一種基于批量梯度下降算法的自適應(yīng)調(diào)參策略。假設(shè)無(wú)人機(jī)實(shí)際角加速度為y，控制量為u，模型預(yù)測(cè)輸出為(u)，為采樣數(shù)量選，取誤差函數(shù)()為：

有：

則參數(shù)：

式中：為修正因子， 0＜＜1。實(shí)際應(yīng)用中由于噪聲的存在，需要對(duì)u和y進(jìn)行低通濾波。

6 仿真分析

本文使用Matlab 的Simulink 搭建此姿態(tài)控制器的仿真模型，結(jié)構(gòu)如圖1 所示，令ESO 觀測(cè)參數(shù)=0.1，=0.001，真實(shí)姿態(tài)模型的=5.5，= 0.1，ESO 和控制的、參數(shù)為真實(shí)的模型參數(shù)，反饋增益參數(shù)= 5，= 15，=50，ESO仿真結(jié)果如下圖所示。

如圖4 和圖5 所示，和原始濾波后的數(shù)據(jù)相比，在同樣的濾波效果前提下，ESO 估計(jì)角速度和ESO 估計(jì)角加速度延時(shí)更小，極大提高了控制器的穩(wěn)定性和魯棒性。擾動(dòng)估計(jì)如圖6 所示， 在0.1 秒處加入階躍擾動(dòng)后，傳統(tǒng)ESO 擾動(dòng)估計(jì)并不準(zhǔn)確，而本文的帶模型ESO 在0.15秒處即可估計(jì)出百分之八十的擾動(dòng)，0.3 秒便達(dá)到了穩(wěn)定狀態(tài)。

圖4 角速度對(duì)比

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

圖6 擾動(dòng)估計(jì)對(duì)比

其他參數(shù)不變，令參數(shù)取值分別為24 和1.5，仿真結(jié)果如圖7 和圖8 所示，可以看到參數(shù)太大會(huì)導(dǎo)致角度控制超調(diào)并大幅度低頻震蕩，參數(shù)太小則會(huì)導(dǎo)致角度控制小幅度高頻震蕩，但都不會(huì)發(fā)散。

圖7 角度控制（b = 24，T = 0.1）

圖8 角度控制 （b = 1.5，T = 0.1）

當(dāng)控制參數(shù)偏離真實(shí)參數(shù)時(shí)，角度控制也會(huì)發(fā)生震蕩，對(duì)于多旋翼無(wú)人機(jī)，實(shí)際應(yīng)用中參數(shù)的范圍一般在0.05～0.2 之間，變化范圍較小，因此通過(guò)調(diào)整參數(shù)也能達(dá)到很好的控制效果，仿真結(jié)果如圖9和圖10所示。

圖9 角度控制（b = 8，T = 0.2）

圖10 角度控制（b = 4，T = 0.05）

7 實(shí)際飛行驗(yàn)證

此姿態(tài)控制算法在數(shù)十臺(tái)多旋翼無(wú)人機(jī)上進(jìn)行了驗(yàn)證， 包括330～1600 mm 軸距、不同動(dòng)力配置的多旋翼無(wú)人機(jī)，實(shí)際測(cè)試中發(fā)現(xiàn)參數(shù)通常為0.1 秒左右，故只需調(diào)整參數(shù)即可， 直觀表現(xiàn)為參數(shù)過(guò)大無(wú)人機(jī)姿態(tài)會(huì)大幅度晃動(dòng)，參數(shù)太小無(wú)人機(jī)姿態(tài)則會(huì)高頻抖動(dòng)，與仿真結(jié)果一致。

圖11 測(cè)試無(wú)人機(jī)

在此控制器一般只需調(diào)整參數(shù)的前提下,為進(jìn)一步提高控制器的魯棒性，引入了自適應(yīng)調(diào)參策略。以330 mm 軸距的四旋翼無(wú)人機(jī)為例進(jìn)行實(shí)飛測(cè)試，其真實(shí)參數(shù)和參數(shù)分別為0.1和5.5左右，設(shè)置初始參數(shù)為1和60，修正因子為0.002，參數(shù)自適應(yīng)效果如圖12所示，估計(jì)參數(shù)在10秒內(nèi)由60收斂到15左右，最后收斂接近于真實(shí)的參數(shù)，有效防止了控制發(fā)散。

圖12 估計(jì)b參數(shù)

8 結(jié)語(yǔ)

針對(duì)多旋翼無(wú)人機(jī)，本文提出了一種自適應(yīng)調(diào)參的自抗擾姿態(tài)控制器。通過(guò)引入一階慣性模型，降低ESO 負(fù)擔(dān)，能夠快速估計(jì)出擾動(dòng)并補(bǔ)償?shù)?，結(jié)合四階線性跟蹤微分器，設(shè)計(jì)反步控制律代替?zhèn)鹘y(tǒng)的非線性PID，仿真和實(shí)際飛行結(jié)果表明，對(duì)于不同軸距和動(dòng)力的無(wú)人機(jī)，一般只需要調(diào)節(jié)參數(shù)即可。為了進(jìn)一步提高調(diào)參的簡(jiǎn)易性，解決因機(jī)身結(jié)構(gòu)、負(fù)載和電池動(dòng)力導(dǎo)致的參數(shù)攝動(dòng)問(wèn)題，控制參數(shù)與實(shí)際參數(shù)相差過(guò)大而導(dǎo)致的控制發(fā)散問(wèn)題，提出了基于批量梯度下降的自適應(yīng)調(diào)參策略，實(shí)際飛行結(jié)果證明控制參數(shù)與實(shí)際參數(shù)相差10倍都能夠迅速收斂并接近真實(shí)參數(shù)， 證明此姿態(tài)控制器具有很高的穩(wěn)定性、魯棒性和實(shí)用性。