四軸飛行器姿態(tài)控制系統(tǒng)設計

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

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

0 引 言

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

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

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

1 四軸飛行器工作原理

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

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

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

表1 四軸飛行器姿態(tài)與電機轉速的關系

2 飛行器姿態(tài)動力學方程與四元數(shù)法

2.1 使用歐拉角微分方程求解飛行器姿態(tài)

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

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

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

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

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

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

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

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

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

2.2 使用四元數(shù)法求解飛行器姿態(tài)

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

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

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

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

設q＝［q0q1q2q3］T為規(guī)范四元數(shù)，則四元數(shù)方程的矩陣形式為：

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

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

則可寫成：

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

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

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

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

式中

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

3 系統(tǒng)整體架構

根據(jù)四軸飛行器系統(tǒng)設計目標及對市場上現(xiàn)有的四軸飛行器分析，采用如圖3所示的飛行器姿態(tài)控制系統(tǒng)設計。

圖3 Picopter四軸飛行器姿態(tài)控制系統(tǒng)結構圖

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

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

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

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

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

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

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

4 系統(tǒng)軟件設計

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

圖4 Picopter飛行姿態(tài)控制軟件架構圖

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

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

圖5 四軸飛行器控制系統(tǒng)框圖

5 實驗結果

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

對飛行器進行的首次試飛測試環(huán)境如下：

（1）系統(tǒng)電源：1S 3.7 V500 mAh鋰電池；

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

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

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

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

6 結 論

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

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

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

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

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

［5］ 陳振興.基于STM32的微型四軸飛行器研究與設計［D］.石家莊：河北工業(yè)大學，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］ 黃國強，陸宇平，南 英.飛行器軌跡優(yōu)化數(shù)值算法綜述［J］.中國科學：技術科學，2012，09（8）：1016－1036.

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

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

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

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