黃吉傳，張 賀，繆存孝

（1.航空工業(yè)成都飛機(jī)工業(yè)（集團(tuán)）公司，成都 610092；2.西北工業(yè)大學(xué)電子信息學(xué)院，西安 710129；3.空軍裝備部駐成都地區(qū)第一軍事代表室，成都 610092；4.北京科技大學(xué)機(jī)械工程學(xué)院，北京 100083）

0 引言

因具有垂直起降、懸停和自主飛行等功能，四旋翼飛行器被廣泛應(yīng)用于軍事和民用等領(lǐng)域，逐漸成為學(xué)術(shù)研究中的一個(gè)熱點(diǎn)［1］。在軍事應(yīng)用上，無(wú)人飛行器可以作為偵察機(jī)或戰(zhàn)斗機(jī)，執(zhí)行戰(zhàn)場(chǎng)偵察、通信中繼、目標(biāo)定位、定點(diǎn)精確轟炸、攔截導(dǎo)彈等重要軍事任務(wù)［2］。在民用方面，無(wú)人飛行器被用于災(zāi)區(qū)搜救、航空拍攝、氣象探測(cè)、電力線路巡查等高空復(fù)雜任務(wù)［3?4］。 然而，四旋翼飛行器是一個(gè)典型的多變量、非線性、強(qiáng)耦合的欠驅(qū)動(dòng)系統(tǒng)，其控制器設(shè)計(jì)十分困難。近年來(lái)，研究人員針對(duì)四旋翼飛行器的建模、控制及工程應(yīng)用開(kāi)展了大量研究工作［5?10］。目前，四旋翼飛行器的飛行控制系統(tǒng)設(shè)計(jì)及工程應(yīng)用是一個(gè)主要的研究方向。四旋翼飛行器的飛行控制系統(tǒng)一般采用內(nèi)外雙環(huán)控制，內(nèi)環(huán)為姿態(tài)控制環(huán)，外環(huán)為位置控制環(huán)，在一些實(shí)際應(yīng)用中，位置控制可使用遠(yuǎn)程遙控，而姿態(tài)控制通常都由機(jī)載系統(tǒng)自主完成，機(jī)體位置的變化也是由姿態(tài)變化引起的，因此姿態(tài)控制是整個(gè)飛行控制系統(tǒng)的基礎(chǔ)和關(guān)鍵［11?12］。四旋翼飛行器在實(shí)際飛行中易受外部干擾，因此設(shè)計(jì)魯棒性強(qiáng)、抗干擾性好的四旋翼飛行器姿態(tài)控制系統(tǒng)是關(guān)鍵。

四旋翼飛行器的控制策略目前分為三大類(lèi)，包括：1）最為常見(jiàn)的PID控制、線性二次型等局部線性控制［13?14］；2）反步法、 滑?？刂啤?自適應(yīng)控制等非線性控制［15?16］；3）神經(jīng)網(wǎng)絡(luò)控制、 模糊控制等智能控制［17?18］。以上方法或?qū)Ψ蔷€性考慮不足，與實(shí)際系統(tǒng)相差較遠(yuǎn)，導(dǎo)致實(shí)際飛行中控制效果不理想；或?qū)δＰ途_度要求較高，在實(shí)際應(yīng)用中的控制效果還有待提高；或是算法較為復(fù)雜，難以實(shí)現(xiàn)。同時(shí)，以上方法的姿態(tài)大多采用Euler角及方向余弦矩陣進(jìn)行表示，以此為基礎(chǔ)建立的姿態(tài)系統(tǒng)模型是局部的、不完備的，甚至有可能出現(xiàn)計(jì)算奇異性，使得需要在奇異點(diǎn)進(jìn)行特殊處理，從而增加了控制器設(shè)計(jì)的難度。近年來(lái)，幾何控制的方法［19?24］逐漸應(yīng)用于四旋翼飛行器控制中，并取得了良好的控制效果。它通過(guò)將四旋翼的姿態(tài)向量映射到特殊正交群SO（3）空間，極大簡(jiǎn)化了四旋翼的模型表示方法，并優(yōu)化了控制器的設(shè)計(jì)過(guò)程。國(guó)外學(xué)者Lee等［19?21］在這方面做了大量的深入研究，先后采用非線性PID、魯棒自適應(yīng)等控制方法設(shè)計(jì)控制器，取得了豐碩的理論成果。國(guó)內(nèi)目前對(duì)基于SO（3）的姿態(tài)控制研究相對(duì)較少，劉錦濤等［22?23］從理論上研究了四旋翼 SO（3）滑模變結(jié)構(gòu)控制、自適應(yīng)控制，并進(jìn)行了仿真實(shí)驗(yàn)，但均未進(jìn)行實(shí)際的實(shí)驗(yàn)驗(yàn)證。 劉超等［24］在SO（3）上設(shè)計(jì)了滑模觀測(cè)器并應(yīng)用于四旋翼平臺(tái)上對(duì)實(shí)際姿態(tài)和旋轉(zhuǎn)角度進(jìn)行觀測(cè)，驗(yàn)證了觀測(cè)器的效果。針對(duì)四旋翼飛行器非線性、強(qiáng)耦合、易受外部干擾的特性，自抗擾控制［25?26］（ADRC）越來(lái)越多地應(yīng)用于四旋翼飛行器控制中，但是基于ADRC的控制器采用非線性函數(shù)，參數(shù)較多、調(diào)節(jié)復(fù)雜，在實(shí)際應(yīng)用中難以簡(jiǎn)單快速實(shí)現(xiàn)。故本文提出了一種基于SO（3）空間的線性自抗擾姿態(tài)控制（LADRC）方法，通過(guò)室內(nèi)姿態(tài)控制實(shí)驗(yàn)和室內(nèi)懸停實(shí)驗(yàn)證明了該方法可以提高四旋翼飛行器姿態(tài)控制系統(tǒng)的抗干擾能力。

1 SO（3）四旋翼飛行器數(shù)學(xué)模型

在剛體的三維空間運(yùn)動(dòng)中，慣性系與剛體本體坐標(biāo)系之間的轉(zhuǎn)換矩陣可以用一個(gè)正交變換矩陣R來(lái)描述，所有的正交變換矩陣構(gòu)成特殊的正交矩陣群：

四旋翼飛行器的姿態(tài)運(yùn)動(dòng)方程可表示為［19?21］

式（1）中，R∈SO（3）為飛行器本體坐標(biāo)系到慣性坐標(biāo)系的旋轉(zhuǎn)矩陣；Ω∈R3為機(jī)體角速度；＾Ω為Ω的hat映射；J∈R3×3為機(jī)體坐標(biāo)系下的慣性矩；M∈R3為控制力矩；ΔR∈R3為姿態(tài)動(dòng)力學(xué)中的未知外部干擾，且為正常數(shù)。

設(shè)x＝［x1x2x3］T， 定 義運(yùn) 算為hat映射，記為∧。 hat映射的逆運(yùn)算為vee映射，記為∨， 定義為將任意三 維 反 對(duì) 稱 陣 映 射 為 三 維 向 量［19?21］， 即

2 基于LADRC的姿態(tài)控制器設(shè)計(jì)

針對(duì)四旋翼飛行器非線性、多變量、強(qiáng)耦合和對(duì)擾動(dòng)敏感等控制問(wèn)題，設(shè)計(jì)了基于線性擴(kuò)張狀 態(tài) 觀 測(cè) 器 （LinearExpansion StateObserver，LESO）的姿態(tài)控制系統(tǒng)，如圖1所示。圖1中，R為目標(biāo)旋轉(zhuǎn)矩陣，u0為PD控制輸出，b0為補(bǔ)償因子，zi（i＝1，2，3）為觀測(cè)值，u為控制器輸出，y為系統(tǒng)輸出。

圖1 姿態(tài)控制系統(tǒng)結(jié)構(gòu)Fig.1 Structure of attitude control system

2.1 線性擴(kuò)張狀態(tài)觀測(cè)器（LESO）設(shè)計(jì)

將力矩矢量M∈R3看作四旋翼飛行器系統(tǒng)的控制輸入量，則式（1）可以改寫(xiě)為

進(jìn)一步可寫(xiě)為

式（3）中，b為等效輸入矩陣，與機(jī)體參數(shù)相關(guān)。

根據(jù)文獻(xiàn)［19］～文獻(xiàn)［21］，定義估計(jì)的機(jī)體坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)換矩陣Rz與機(jī)體坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)換矩陣R的誤差為

設(shè)計(jì)離散遞推式的線性擴(kuò)張狀態(tài)觀測(cè)器（LESO）為

式（5）中，zi（i＝1，2，3）為觀測(cè)值；h為采樣周期；βi（i＝1，2，3）為觀測(cè)值增益，并且β1＝為觀測(cè)器帶寬。

2.2 線性反饋控制律（LSEF）設(shè)計(jì)

四旋翼飛行器的姿態(tài)控制采用內(nèi)外雙環(huán)控制，外環(huán)為位置控制產(chǎn)生的姿態(tài)角設(shè)定值，輸出的是角速度設(shè)定值，而內(nèi)環(huán)則根據(jù)輸入的角速度設(shè)定值進(jìn)行控制?；贚ADRC的姿態(tài)控制器結(jié)構(gòu)如圖2所示［27］，由于LESO能夠?qū)崟r(shí)估計(jì)并補(bǔ)償內(nèi)部與外部擾動(dòng)，因此傳統(tǒng)PID中在常值擾動(dòng)下為消除靜差而采用的積分器已不再必要，線性狀態(tài)誤差反饋控制率進(jìn)一步簡(jiǎn)化為PD組合的設(shè)計(jì)。

圖2 基于LADRC的姿態(tài)控制器Fig.2 Attitude controller based on LADRC

估計(jì)的機(jī)體坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)換矩陣Rz與期望的機(jī)體坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)換矩陣Rd的誤差為

式（6）中，Rz＝z1。

角度環(huán)采用比例控制，控制率為

角速度環(huán)線性反饋控制率為

式 （8）中， 角 速 度 誤 差 為rates_err＝rates_sp?z2，kp、kd分別為比例（P）和微分（D）的放大系數(shù)。

則擾動(dòng)補(bǔ)償過(guò)程為

3 實(shí)驗(yàn)驗(yàn)證及結(jié)果分析

對(duì)所設(shè)計(jì)的控制方法在實(shí)際四旋翼飛行平臺(tái)中進(jìn)行實(shí)驗(yàn)驗(yàn)證，室內(nèi)飛行平臺(tái)由四部分組成：地面站、通信系統(tǒng)、四旋翼飛行器以及Optitrack室內(nèi)光學(xué)定位系統(tǒng)，實(shí)驗(yàn)平臺(tái)結(jié)構(gòu)如圖3所示，其實(shí)驗(yàn)環(huán)境如圖4所示?；赟imulink編寫(xiě)地面站，通過(guò)Motive定位解算軟件獲取四旋翼飛行器的位置和姿態(tài)數(shù)據(jù)，采用Mavlink通信協(xié)議編寫(xiě)串口通信模塊，通過(guò)Wifi實(shí)現(xiàn)地面站與飛控間的通信，機(jī)載控制板采用開(kāi)源飛行控制器Pixhawk，四旋翼機(jī)架為軸距280mm的碳纖維機(jī)架，質(zhì)量小且強(qiáng)度高。

圖3 實(shí)驗(yàn)平臺(tái)結(jié)構(gòu)Fig.3 Structure of experiment platform

圖4 實(shí)驗(yàn)環(huán)境Fig.4 Schematic diagram of experiment environment

3.1 姿態(tài)控制實(shí)驗(yàn)

本文研究的重點(diǎn)是對(duì)飛行器姿態(tài)的控制，通過(guò)輔助的四旋翼固定架將飛行器的位置運(yùn)動(dòng)自由度鎖定。如圖5所示，四旋翼飛行器固定于帶有3?DOF旋轉(zhuǎn)關(guān)節(jié)（萬(wàn)向節(jié)）的基座上，從而使得飛行器只有3個(gè)姿態(tài)運(yùn)動(dòng)的自由度。

圖5 姿態(tài)控制測(cè)試平臺(tái)Fig.5 Experiment platform of attitude control

由于實(shí)驗(yàn)中采用Optitrack室內(nèi)光學(xué)定位系統(tǒng)可以獲得四旋翼的高精度位姿信息，所以實(shí)際控制中角度誤差計(jì)算采用

角速度誤差計(jì)算采用

分別進(jìn)行3種情形下的實(shí)驗(yàn)驗(yàn)證：

實(shí)驗(yàn)一：期望姿態(tài)角均為0°時(shí)，施加干擾力矩，并與PID控制方法進(jìn)行對(duì)比。

實(shí)驗(yàn)二：期望姿態(tài)角為正弦信號(hào)時(shí)，不施加干擾，并與PID控制方法進(jìn)行對(duì)比。

實(shí)驗(yàn)三：期望姿態(tài)角為正弦信號(hào)時(shí)，施加干擾力矩，并與PID控制方法進(jìn)行對(duì)比。

（1）實(shí)驗(yàn)一

實(shí)驗(yàn)一中設(shè)定的期望姿態(tài)為θ＝0°、φ＝0°，穩(wěn)定飛行過(guò)程中在俯仰和橫滾方向突然施加干擾力矩（以掛載200g砝碼實(shí)現(xiàn)），PID控制與本文算法的實(shí)驗(yàn)結(jié)果分別如圖6和圖7所示?？梢钥闯觯琍ID控制在靜態(tài)條件下θ的穩(wěn)態(tài)偏差為0.98°左右，φ的穩(wěn)態(tài)偏差為1.23°左右，受到外界干擾后姿態(tài)角出現(xiàn)了10°左右的偏差；本文算法在靜態(tài)條件下θ的穩(wěn)態(tài)偏差為0.50°左右，φ的穩(wěn)態(tài)偏差為0.33°左右，受到外界干擾后姿態(tài)角出現(xiàn)4°左右的偏差。

圖6 靜態(tài)抗干擾下的俯仰角曲線Fig.6 Curves of pitch angle under static anti?interference

圖7 靜態(tài)抗干擾下的橫滾角曲線Fig.7 Curves of roll angle under static anti?interference

可見(jiàn)，靜態(tài)條件下本文設(shè)計(jì)的基于干擾觀測(cè)器的姿態(tài)控制器比PID控制更穩(wěn)定，同時(shí)在系統(tǒng)遭遇擾動(dòng)時(shí)，干擾造成的輸出波動(dòng)幅值變化更小，平均降低約50%，表現(xiàn)出了更強(qiáng)的抗干擾能力，說(shuō)明系統(tǒng)具有較好的自適應(yīng)性。

（2）實(shí)驗(yàn)二

圖8 動(dòng)態(tài)跟蹤下的俯仰角曲線Fig.8 Curves of pitch angle under dynamic tracking

圖9 動(dòng)態(tài)跟蹤下的橫滾角曲線Fig.9 Curves of roll angle under dynamic tracking

（3）實(shí)驗(yàn)三

圖10 動(dòng)態(tài)抗干擾下的俯仰角曲線Fig.10 Curves of pitch angle under dynamic anti?interference

圖11 動(dòng)態(tài)抗干擾下的橫滾角曲線Fig.11 Curves of roll angle under dynamic anti?interference

3.2 懸停實(shí)驗(yàn)

懸停實(shí)驗(yàn)采用Optitrack室內(nèi)光學(xué)定位系統(tǒng)獲得機(jī)體的空間位置，位置控制方法采用串聯(lián)PID控制，位置環(huán)采用P控制，速度環(huán)采用PID控制，姿態(tài)控制方法采用上述線性自抗擾控制，期望位姿設(shè)置為：（0m，0m，-1m，0°）， 高度方向的跟蹤曲線如圖12所示。

圖12 懸停實(shí)驗(yàn)高度跟蹤曲線Fig.12 Height tracking curve in hover experiment

由圖12可知，懸停時(shí)高度誤差小于3cm，間接驗(yàn)證了本文提出的線性自抗擾姿態(tài)控制方法的有效性。

4 結(jié)論

本文提出了一種基于SO（3）空間的線性自抗擾姿態(tài)控制（LADRC）方法，可得如下結(jié)論：

1）靜態(tài)條件下，θ和φ的穩(wěn)態(tài)偏差小于1°，受到外界干擾后姿態(tài)角可以5s內(nèi)迅速恢復(fù)；

2）動(dòng)態(tài)條件下，θ和φ可以精確快速跟蹤設(shè)定值，當(dāng)存在外界干擾時(shí)，θ和φ仍然可以快速跟蹤設(shè)定值，在幅值處出現(xiàn)1°左右的跟蹤誤差；

3）懸停時(shí)采用本文提出的姿態(tài)控制方法，懸停精度優(yōu)于3cm；

4）實(shí)驗(yàn)表明，在靜動(dòng)態(tài)條件下，本文所提出的姿態(tài)控制器可提高姿態(tài)跟蹤的魯棒性，有很強(qiáng)的抗干擾能力，能夠滿足四旋翼無(wú)人機(jī)的姿態(tài)控制要求，具有實(shí)際應(yīng)用價(jià)值。