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

常 敏，崔永進(jìn)，何蓓薇，張學(xué)典，錢(qián)研華，王 戈

（上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院，上海 200093）

0 引 言

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

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

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

1 四軸飛行器工作原理

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

圖1 四軸飛行器結(jié)構(gòu)示意圖

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

表1 四軸飛行器姿態(tài)與電機(jī)轉(zhuǎn)速的關(guān)系

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

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

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

圖2 地球坐標(biāo)系與機(jī)體坐標(biāo)系關(guān)系圖

設(shè)ω1是機(jī)體坐標(biāo)系OX1Y1Z1相對(duì)于地球坐標(biāo)系OX0Y0Z0的旋轉(zhuǎn)角速度，它在機(jī)體坐標(biāo)系OX1Y1Z1三軸上的分量為ωX1，ωY1，ωZ1，則：

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

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

但式（2）存在如下兩個(gè)問(wèn)題：

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

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

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

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

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

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

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

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

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

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

為了書(shū)寫(xiě)方便，引入符號(hào)：

則可寫(xiě)成：

這是一階四維標(biāo)量微分方程，其標(biāo)量之間滿足正交關(guān)系式：

如果利用一階差分法解標(biāo)量方程時(shí)，計(jì)算步長(zhǎng)Δtk，則

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

若采用四階龍格－庫(kù)塔法計(jì)算時(shí)，則：

式中

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

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

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

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

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

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

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

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

（4）直流電機(jī)驅(qū)動(dòng)：接收來(lái)自主控芯片的PWM控制信號(hào)，調(diào)節(jié)固定在飛行器四端的空心杯直流電機(jī)轉(zhuǎn)速。

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

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

4 系統(tǒng)軟件設(shè)計(jì)

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

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

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

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

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

5 實(shí)驗(yàn)結(jié)果

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

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

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

（2）無(wú)線連接：藍(lán)牙v2.1 SPP Profile；

（3）測(cè)試環(huán)境：在室內(nèi)無(wú)風(fēng)環(huán)境，進(jìn)行簡(jiǎn)單飛行，完成滯空、傾斜等基本飛行動(dòng)作。

圖6 四軸飛行器的飛行測(cè)試圖

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

6 結(jié) 論

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

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

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

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

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

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

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

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

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

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