黃吉傳，張 賀，繆存孝

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

進一步可寫為

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

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

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

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

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

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

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

估計的機體坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)換矩陣Rz與期望的機體坐標(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ù)。

則擾動補償過程為

3 實驗驗證及結(jié)果分析

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

圖3 實驗平臺結(jié)構(gòu)Fig.3 Structure of experiment platform

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

3.1 姿態(tài)控制實驗

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

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

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

角速度誤差計算采用

分別進行3種情形下的實驗驗證：

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

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

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

（1）實驗一

實驗一中設(shè)定的期望姿態(tài)為θ＝0°、φ＝0°，穩(wěn)定飛行過程中在俯仰和橫滾方向突然施加干擾力矩（以掛載200g砝碼實現(xiàn)），PID控制與本文算法的實驗結(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

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

（2）實驗二

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

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

（3）實驗三

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

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

3.2 懸停實驗

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

圖12 懸停實驗高度跟蹤曲線Fig.12 Height tracking curve in hover experiment

由圖12可知，懸停時高度誤差小于3cm，間接驗證了本文提出的線性自抗擾姿態(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）動態(tài)條件下，θ和φ可以精確快速跟蹤設(shè)定值，當(dāng)存在外界干擾時，θ和φ仍然可以快速跟蹤設(shè)定值，在幅值處出現(xiàn)1°左右的跟蹤誤差；

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

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