基于SO（3）空間的線性自抗擾姿態控制方法

黃吉傳，張 賀，繆存孝

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

0 引言

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

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

1 SO（3）四旋翼飛行器數學模型

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

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

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

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

2 基于LADRC的姿態控制器設計

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

圖1 姿態控制系統結構Fig.1 Structure of attitude control system

2.1 線性擴張狀態觀測器（LESO）設計

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

進一步可寫為

式（3）中，b為等效輸入矩陣，與機體參數相關。

根據文獻［19］～文獻［21］，定義估計的機體坐標系到慣性坐標系的轉換矩陣Rz與機體坐標系到慣性坐標系的轉換矩陣R的誤差為

設計離散遞推式的線性擴張狀態觀測器（LESO）為

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

2.2 線性反饋控制律（LSEF）設計

四旋翼飛行器的姿態控制采用內外雙環控制，外環為位置控制產生的姿態角設定值，輸出的是角速度設定值，而內環則根據輸入的角速度設定值進行控制。基于LADRC的姿態控制器結構如圖2所示［27］，由于LESO能夠實時估計并補償內部與外部擾動，因此傳統PID中在常值擾動下為消除靜差而采用的積分器已不再必要，線性狀態誤差反饋控制率進一步簡化為PD組合的設計。

圖2 基于LADRC的姿態控制器Fig.2 Attitude controller based on LADRC

估計的機體坐標系到慣性坐標系的轉換矩陣Rz與期望的機體坐標系到慣性坐標系的轉換矩陣Rd的誤差為

式（6）中，Rz＝z1。

角度環采用比例控制，控制率為

角速度環線性反饋控制率為

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

則擾動補償過程為

3 實驗驗證及結果分析

對所設計的控制方法在實際四旋翼飛行平臺中進行實驗驗證，室內飛行平臺由四部分組成：地面站、通信系統、四旋翼飛行器以及Optitrack室內光學定位系統，實驗平臺結構如圖3所示，其實驗環境如圖4所示。基于Simulink編寫地面站，通過Motive定位解算軟件獲取四旋翼飛行器的位置和姿態數據，采用Mavlink通信協議編寫串口通信模塊，通過Wifi實現地面站與飛控間的通信，機載控制板采用開源飛行控制器Pixhawk，四旋翼機架為軸距280mm的碳纖維機架，質量小且強度高。

圖3 實驗平臺結構Fig.3 Structure of experiment platform

圖4 實驗環境Fig.4 Schematic diagram of experiment environment

3.1 姿態控制實驗

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

圖5 姿態控制測試平臺Fig.5 Experiment platform of attitude control

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

角速度誤差計算采用

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

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

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

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

（1）實驗一

實驗一中設定的期望姿態為θ＝0°、φ＝0°，穩定飛行過程中在俯仰和橫滾方向突然施加干擾力矩（以掛載200g砝碼實現），PID控制與本文算法的實驗結果分別如圖6和圖7所示。可以看出，PID控制在靜態條件下θ的穩態偏差為0.98°左右，φ的穩態偏差為1.23°左右，受到外界干擾后姿態角出現了10°左右的偏差；本文算法在靜態條件下θ的穩態偏差為0.50°左右，φ的穩態偏差為0.33°左右，受到外界干擾后姿態角出現4°左右的偏差。

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

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

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

（2）實驗二

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

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

（3）實驗三

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

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

3.2 懸停實驗

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

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

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

4 結論

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

1）靜態條件下，θ和φ的穩態偏差小于1°，受到外界干擾后姿態角可以5s內迅速恢復；

2）動態條件下，θ和φ可以精確快速跟蹤設定值，當存在外界干擾時，θ和φ仍然可以快速跟蹤設定值，在幅值處出現1°左右的跟蹤誤差；

3）懸停時采用本文提出的姿態控制方法，懸停精度優于3cm；

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