四軸飛行器姿態控制系統設計

常 敏，崔永進，何蓓薇，張學典，錢研華，王 戈

（上海理工大學 光電信息與計算機工程學院，上海 200093）

0 引 言

無人駕駛飛行器（UAV）是一種依靠無線遠程遙控或由自身程序控制的不載人飛行器［1］。相比載人飛行器，無人駕駛飛行器具有造價低、體積小、安全性高、使用方便等特點。無人駕駛飛行器可裝備武器系統、傳感器系統、電子干擾系統以及攝像機等設備，分別執行敵方縱深目標打擊、空中偵察、電子干擾以及情報搜集等工作。自誕生起無人駕駛飛行器就受到全世界的廣泛關注，各國爭相設計不同規模、不同用途的無人駕駛飛行器［2］。四軸飛行器在保留了傳統旋翼無人機垂直起降和能夠空中懸停等優點的基礎上，大幅簡化了飛行器的機械結構，使其造價更低、可靠性更高［3］。最初，四軸飛行器是由航模愛好者自制成功，隨后很多自動化廠商發現其可以用于多種用途故積極投入研發。

相比其他類型的無人駕駛飛行器，四軸飛行器體積更為小巧、機動更為靈活、重量更輕且便于攜帶和使用。四軸飛行器配合飛行控制系統，可以實現如地貌勘測、災后搜救、遙感測繪以及邊境巡邏等復雜、危險環境下的特殊飛行任務。故四軸飛行器有廣泛的應用領域以及可觀的商業價值。

本文設計并實現一套成本低廉、性能優異且可靠性較高的四軸飛行器飛行控制系統，選用ARM Cortex M4架構的STM32F303CB單片機作為飛行控制系統的主控芯片，軟件則采用基于ChibiOS實時操作系統（RTOS）的搶占式多任務實時系統。

1 四軸飛行器工作原理

現代四軸飛行器主要依靠四個固定于飛行器機架電機臂末端的電動機來提供飛行所需的動力［4］。其機械結構如圖1所示，圖中四個電機中M1、M3配置為順時針方向旋轉，電機 M2、M4配置為逆時針方向旋轉。順時針方向旋轉與逆時針方向旋轉的電機分別安裝正、反向螺旋槳，以保證電機旋轉過程中產生的推力均為升力。因為相鄰兩個電機的旋轉方向相反，故機身上的四個螺旋槳同時旋轉時產生的水平方向反作用力恰好相互抵消，保證飛機在懸停或平動狀態下不會發生自旋［5］。通過改變四個電機的轉速，四軸飛行器可以實現靈活多變的機動動作。

圖1 四軸飛行器結構示意圖

以圖1為例，當四個電機轉速同步增減時，可以實現飛行器在垂直方向上的升降。對角線上的兩個電機轉速提高會使四軸飛行器水平旋轉，如提高M1、M3轉速會使飛行器向左旋轉，而提高 M2、M4轉速則會使飛行器向右旋轉。調節前后兩組電機的轉速則可以實現飛行器俯仰角度的調節。同理，調節左右兩組電機的轉速可以實現飛行器的橫滾運動。

表1 四軸飛行器姿態與電機轉速的關系

2 飛行器姿態動力學方程與四元數法

2.1 使用歐拉角微分方程求解飛行器姿態

為了解出飛行器在飛行狀態下的歐拉角Ψ、Φ、γ需要建立歐拉角速度方程。

圖2 地球坐標系與機體坐標系關系圖

設ω1是機體坐標系OX1Y1Z1相對于地球坐標系OX0Y0Z0的旋轉角速度，它在機體坐標系OX1Y1Z1三軸上的分量為ωX1，ωY1，ωZ1，則：

根據圖2可得歐拉角速度方程如下：

此方程組為非線性變系數微分方程組，如果知道瞬間角速度ωX1、ωY1、ωZ1，則在給定的起始條件下，解方程組即可求出姿態角Ψ、φ、γ［6］。

但式（2）存在如下兩個問題：

a.方程組為非線性變系數微分方程，只能使用數值積分法求解，計算量大［7］。

b.在大姿態角情況下，方程是奇異的［8］。

為了解決上述問題，往往要使用四元數法等其他數學工具來求解飛行器的姿態變化。

2.2 使用四元數法求解飛行器姿態

四元數是在1843年由哈密頓首次提出的。但一直沒有得到任何實際應用。近些年隨著捷聯式慣性導航技術的發展，為彌補歐拉角參數計算量大和大姿態角條件下方程退化的不足，四元數這個數學工具常被用來描述剛體角運動［9］。四元數可以描述一個矢量或一個坐標系相對另一坐標系的旋轉。

四元數是由一個實數單位和三個虛數單位i、j、k組成的超復數，通常寫成：

式中，λ0、λ1、λ2、λ3均為實數，i、j、k為三位空間中一組相互正交的單位矢量。

為了能用四元數來研究飛行器的姿態變化，需要建立飛行器旋轉角速度與四元數導數之間的關系以建立四元數方程［10］。

設q＝［q0q1q2q3］T為規范四元數，則四元數方程的矩陣形式為：

工程上常采用差分法或龍格－庫塔法求解四元數方程。

為了書寫方便，引入符號：

則可寫成：

這是一階四維標量微分方程，其標量之間滿足正交關系式：

如果利用一階差分法解標量方程時，計算步長Δtk，則

＝［q0k＋1q1k＋1q2k＋1q3k＋1］為tk＋1時刻的瞬時四元數。

若采用四階龍格－庫塔法計算時，則：

式中

因差分法和龍格－庫塔法均不能保證四元數的正規性，即不能保證N（q）＝1。為了保證計算的精確性，在計算時必須對四元數進行范數修正［11］。

3 系統整體架構

根據四軸飛行器系統設計目標及對市場上現有的四軸飛行器分析，采用如圖3所示的飛行器姿態控制系統設計。

圖3 Picopter四軸飛行器姿態控制系統結構圖

四軸飛行器姿態控制系統主要由：STM32F3xx單片機、三軸陀螺儀、三軸加速度計、空心杯直流電機驅動、Bluetooth BLE無線通信模塊和電源管理模塊等組成。

（1）STM32F3xx單片機：主要負責通過IIC總線采集陀螺儀和重力加速度計輸出的傳感器數據，通過姿態解算算法求解飛行姿態，并根據來自Bluetooth的控制信號及控制律調節固定在飛行器四端的直流電機轉速。

（2）三軸陀螺儀：用于對飛行器飛行過程中俯仰、橫滾、偏航三個方向的角速度進行測量。通過分別對三個方向角速度進行積分，可以得到飛行器飛行過程中的姿態角。

（3）三軸加速度計：用于對飛行器飛行過程中沿機身前進方向、機身垂直方向和機身側向的加速度進行測量。根據測得的靜態（或勻速狀態）三軸加速度值可以求取飛行器的靜態（準靜態）姿態角。靜態（準靜態）姿態角用于矯正由陀螺儀測量并積分得到的姿態角。

（4）直流電機驅動：接收來自主控芯片的PWM控制信號，調節固定在飛行器四端的空心杯直流電機轉速。

（5）Bluetooth SPP無線通信模塊：用于建立手持端與飛行器間的無線數據連接，手持端通過藍牙發送控制信號給飛行器，同時飛行器也通過藍牙傳輸飛行器姿態、電池剩余電量和電機轉速等監控信息。

（6）電源管理模塊：用于為飛行器其他功能模塊提供電能并提供鋰電池電量管理。該電源模塊被設計成可單獨為MCU及藍牙SPP模塊、空心杯直流電機及其驅動以及IMU傳感器模塊提供電能。同時該模塊所包含的鋰電池電量管理模塊能實現鋰電池充電及電池電量監測功能。

4 系統軟件設計

Picopter四軸飛行器飛行姿態控制軟件主要由STM32F303CB片內外設硬件抽象層驅動（HAL Driver）、ChibiOS實時操作系統內核（RTOS kernel）、板級支持包（BSP）、模塊化的中間件和函數庫以及上層應用組成。飛行姿態控制軟件架構如圖4所示。

圖4 Picopter飛行姿態控制軟件架構圖

如圖4所示，Picopter四軸飛行器的飛行姿態控制軟件主要分成三層：硬件抽象層與實時操作系統、模塊化的中間件與函數庫以及上層應用軟件。

飛行控制軟件的主要功能是讀取來自手持端的控制信號、按設定好的周期完成姿態解算任務和PID控制任務，并結合從手持端接收到的油門值和PID控制器輸出的俯仰、橫滾、偏航三個修正值以PWM的形式控制飛行器四個電機的轉速以控制飛行器的飛行姿態。飛行器的控制系統框圖如圖5所示。

圖5 四軸飛行器控制系統框圖

5 實驗結果

在完成了系統軟硬件設計及相關算法的研究后，對課題所研究的四軸飛行器進行了調試，以測試飛行器的基本功能。其中控制參數只做了粗略的設定，為得到更精確的控制參數需要對飛行器進行反復的調試和深入的研究。

對飛行器進行的首次試飛測試環境如下：

（1）系統電源：1S 3.7 V500 mAh鋰電池；

（2）無線連接：藍牙v2.1 SPP Profile；

（3）測試環境：在室內無風環境，進行簡單飛行，完成滯空、傾斜等基本飛行動作。

圖6 四軸飛行器的飛行測試圖

四軸飛行器的飛行測試如圖6所示，測試結果表明：本課題設計的飛行器能實現在Android手持端的控制下完成一系列基本的飛行動作。但由于整個飛行器系統較為復雜，為了實現更穩定的飛行，還需要對飛行控制系統進行更深入的調校和測試。

6 結 論

本文根據四軸飛行器的結構特點，設計并實現了一套四軸飛行器姿態控制系統。在研究過程中，通過對比現有四軸飛行器姿態控制系統，分析四軸飛行器的動力學特性以及研究其姿態解算方法和控制方法，最終設計并實現了一套完整的四軸飛行器姿態控制系統。

［1］ 史頡華.小型無人駕駛飛行器自主飛行控制研究［D］.南京：南京航空航天大學，2012.

［2］ 李姝麗.無人駕駛飛機飛行仿真與智能控制的研究［D］.大連：大連海事大學，2004.

［3］ 萬 俊.四軸飛行器的研究與制作［D］.武漢：華中師范大學，2014.

［4］ 胡琦逸.四旋翼飛行器的姿態估計與優化控制研究［D］.杭州：杭州電子科技大學，2014.

［5］ 陳振興.基于STM32的微型四軸飛行器研究與設計［D］.石家莊：河北工業大學，2014.

［6］ CASTILLO P，DZUL A，LOZANO R.Real－time stabilization and tracking of a four rotor mini rotorcraft［J］.IEEE transactions on control systems technology，2004，12（7）：25－31.

［7］ 黃國強，陸宇平，南 英.飛行器軌跡優化數值算法綜述［J］.中國科學：技術科學，2012，09（8）：1016－1036.

［8］ 李躍軍，閻 超.飛行器姿態角解算的全角度雙歐法［J］.北京航空航天大學學報，2007，05（7）：505－508.

［9］ 張榮輝，賈宏光，陳 濤，張 躍.基于四元數法的捷聯式慣性導航系統的姿態解算［J］.光學精密工程，2008，10（8）：1963－1970.

［10］項鳳濤，王正志，吳第旻，岳 達.捷聯系統四元數姿態解算的精細積分法［J］.四川兵工學報，2010，05（9）：103－106.

［11］周江華，苗育紅，李 宏，孫國基.四元數在剛體姿態仿真中的應用研究［J］.飛行力學，2000，04（8）：28－32.