0 引言

    四旋翼飛行器體積小、重量輕，與載人飛機(jī)相比成本很低，可廣泛應(yīng)用在交通檢測、園林維護(hù)、災(zāi)區(qū)搜救等場合，目前在國內(nèi)外正處于熱門研究中^[1-3]。但四旋翼飛行器是一個非線性、多變量、高度耦合、欠驅(qū)動系統(tǒng)（6個自由度，4個輸入量），控制系統(tǒng)較為復(fù)雜，系統(tǒng)抗干擾能力差。只有采用了合適的控制方案才能夠?qū)崿F(xiàn)平穩(wěn)的飛行，這就需要對飛行器進(jìn)行力學(xué)和運(yùn)動學(xué)上的分析并建立相對應(yīng)的數(shù)學(xué)模型，設(shè)計單片機(jī)控制電路及軟件控制系統(tǒng)等，實現(xiàn)對飛行器的穩(wěn)定控制。

1 四旋翼飛行器數(shù)學(xué)模型建立

    為簡化分析，假設(shè)以下條件成立：

    （1）機(jī)體坐標(biāo)系的原點(diǎn)為飛行器的質(zhì)心，并且與飛行器幾何中心重合；

    （2）只考慮螺旋槳產(chǎn)生的氣流，而空氣流動速度及空氣阻力忽略不計；

    （3）飛行器機(jī)體與螺旋槳均為剛體結(jié)構(gòu)，機(jī)體的幾何構(gòu)造及質(zhì)量對稱；

    （4）螺旋槳旋轉(zhuǎn)時產(chǎn)生的反扭矩與轉(zhuǎn)速的平方成正比，螺旋槳產(chǎn)生的升力與轉(zhuǎn)速的平方成正比。

    根據(jù)以上條件，設(shè)F為飛行器受到的外力和，m為飛行器質(zhì)量，V為飛行速度，M為飛行器所受力矩之和，H是飛行器相對于導(dǎo)航坐標(biāo)系的相對動量矩，受力分析如圖1所示。根據(jù)牛頓-歐拉方程對四旋翼飛行器平移運(yùn)動與旋轉(zhuǎn)運(yùn)動建立模型^[4-7]。

    根據(jù)歐拉方程則有三軸力矩平衡方程式：

    由于姿態(tài)穩(wěn)定控制不需對位置和高度進(jìn)行控制，只需考慮角度控制即可，穩(wěn)定控制時姿態(tài)變化較小，可忽略空氣阻力的影響，這樣得到簡化后的動力學(xué)模型為：

    從以上公式可以看出，在四旋翼飛行器的姿態(tài)控制模型中，對任一個角度的控制均可采用PID控制器對其進(jìn)行控制。

2 硬件電路設(shè)計

2.1 硬件系統(tǒng)主要組成

    系統(tǒng)硬件主要包括飛行架構(gòu)、微控制器最小系統(tǒng)、無線通信系統(tǒng)、動力系統(tǒng)、慣性測量單元、電源等幾個部分。其中飛行架構(gòu)是其他幾部分的載體，微控制器最小系統(tǒng)用于數(shù)據(jù)融合、姿態(tài)解算與調(diào)節(jié)，控制協(xié)調(diào)各模塊的工作。無線通信系統(tǒng)用于遙控飛行器和接收數(shù)據(jù)；動力系統(tǒng)由驅(qū)動器、電機(jī)、螺旋槳組成，為飛行器提供升力和推動力；慣性測量及系統(tǒng)為飛行器提供姿態(tài)和高度等信息，是飛行器系統(tǒng)的重要組成部分。

2.2 主要電路原理圖

    系統(tǒng)主要電路原理圖如圖2所示。采用STM32F103C8T6高速單片機(jī)作為主控制單元，MPU6050和HMC5883L組成慣性測量及姿態(tài)檢測單元，實現(xiàn)對三軸的加速度、三軸陀螺儀、三軸磁場方向的檢測，輸出原始數(shù)據(jù)給微控制器，進(jìn)行融合姿態(tài)解算后得到橫滾角、俯仰角、偏航角。采用空心杯直流電機(jī)，使用PWM進(jìn)行調(diào)速。驅(qū)動電路由N溝道的場效應(yīng)管形成的開關(guān)電路實現(xiàn)，選擇漏源電阻較小的場效應(yīng)管，防止電機(jī)輸入電壓變小造成電機(jī)轉(zhuǎn)速的范圍變窄，便于對電機(jī)轉(zhuǎn)速調(diào)節(jié)的控制。

3 軟件系統(tǒng)設(shè)計

3.1 姿態(tài)解算與卡爾曼濾波^[8-12]

    飛行器在運(yùn)行過程中，CPU通過傳感器MPU6050內(nèi)部的三軸加速度傳感器、三軸陀螺儀來實時采集當(dāng)前各方向姿態(tài)數(shù)據(jù)，通過解算后得到三個方向的角度、角速度值，然后調(diào)節(jié)各電機(jī)轉(zhuǎn)速，使其達(dá)到期望姿態(tài)。加速度傳感器用來測量x、y、z三個方向的加速度值，然后與重力加速度比較即可算出三個方向的角度值，但是存在一定誤差。三軸陀螺儀傳感器用來測量x、y、z三個方向角速度，乘以時間即為角度。

    通過陀螺儀計算出來的角度存在一定的誤差，多次累加以后累積誤差越來越大，最終導(dǎo)致計算出的角度與實際角度相差很大。本文的解決方法是把加速度計讀出的角度與陀螺儀計算出的角度相結(jié)合，通過卡爾曼濾波計算出更為準(zhǔn)確的角度值。

3.2 四旋翼飛行器PID控制器設(shè)計^[13-15]

    通過傳感器準(zhǔn)確獲取角度、角速度數(shù)據(jù)后，再與期望姿態(tài)角進(jìn)行對比，實現(xiàn)對飛行器姿態(tài)的調(diào)節(jié)控制?？刂葡到y(tǒng)采用位置式PID控制器進(jìn)行調(diào)節(jié)，四旋翼飛行器主要包括橫滾角、俯仰角、偏航角3種姿態(tài)，只有對3個姿態(tài)角可控，才能實現(xiàn)對四旋翼飛行器的懸停、前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)等控制，因此需設(shè)計三路串級PID控制器，其框圖如圖3所示。

    在設(shè)計中，外環(huán)采用角度環(huán)進(jìn)行PI控制，內(nèi)環(huán)采用角速度環(huán)進(jìn)行D控制。設(shè)置這樣的串級PID可以使飛行器在大角度偏差時修正速度快，小角度偏差時修正速度慢。外環(huán)期望角度為設(shè)定值，如懸停的期望值為0°，而實際角度值由卡爾曼濾波后的角度數(shù)據(jù)提供。外環(huán)PI控制器的輸出值作為內(nèi)環(huán)D控制器的期望值，內(nèi)環(huán)角速度實際值由傳感器MPU6050內(nèi)部的陀螺儀提供。三路PID控制器的輸出分別為roll_out、pitch_out、yaw_out，分別以PWM的形式輸出給4個電機(jī)驅(qū)動電路，每個電機(jī)PWM線性組合如下：

    Y軸前端電機(jī)：Throttle-roll_out-yaw_out；

    Y軸后端電機(jī)：Throttle+pitch_out-yaw_out；

    X軸右端電機(jī)：Throttle-roll_out+yaw_out；

    X軸左端電機(jī)：Throttle+roll_out+yaw_out。

    其中Throttle為電機(jī)輸入固定值，也稱油門。為保證飛行器穩(wěn)定，內(nèi)環(huán)控制器的采樣頻率為200 Hz，外環(huán)控制器采樣頻率50 Hz即可。為保證采樣時間間隔均勻，PID控制器采樣周期由高優(yōu)先級定時器中斷提供。串級PID調(diào)節(jié)、卡爾曼濾波軸波形圖如圖4所示。由圖中可以看出飛行器在受到環(huán)境干擾時，通過串級PID調(diào)節(jié)可迅速讓機(jī)身恢復(fù)至平衡位置。恢復(fù)時間很短，基本無振蕩現(xiàn)象，說明飛行器在串級PID調(diào)節(jié)下具有較強(qiáng)的抗干擾能力，驗證了PID調(diào)節(jié)系統(tǒng)設(shè)計的可靠性。

4 系統(tǒng)測試

4.1 姿態(tài)角測試

    四旋翼飛行器采集到加速度計和陀螺儀的數(shù)據(jù)后，經(jīng)過數(shù)據(jù)融合、卡爾曼濾波將兩種數(shù)據(jù)融合在一起計算出較為精確的姿態(tài)角數(shù)據(jù)，再將數(shù)據(jù)通過串口發(fā)送到由匿名科創(chuàng)提供的上位機(jī)軟件，將三軸姿態(tài)角用3D效果圖直觀顯示出來。其中橫滾角姿態(tài)解算3D效果如圖5所示，由圖中可以看出系統(tǒng)響應(yīng)穩(wěn)定、姿態(tài)平穩(wěn)，說明系統(tǒng)能很好地實現(xiàn)這些姿態(tài)控制。

4.2 平衡飛行測試

    將飛行器半固定在水平桿上調(diào)試內(nèi)外環(huán)PID各項系數(shù)，在調(diào)試俯仰這一路的PID控制器時，先將K_i和K_p設(shè)為零，K_p增大到控制回路的輸出出現(xiàn)臨界振蕩較為合適，當(dāng)選取到12左右時，飛行器在上升過程中出現(xiàn)上下擺動的不穩(wěn)定現(xiàn)象，通過設(shè)置K_d參數(shù)抵消比例作用調(diào)節(jié)時的過度反應(yīng)和過沖干擾，使飛行器系統(tǒng)在上升時能夠趨于穩(wěn)定飛行，K_d取0.05左右就可以滿足要求，為消除靜態(tài)誤差，K_i參數(shù)可以選取0.02左右。經(jīng)反復(fù)調(diào)試達(dá)到穩(wěn)定狀態(tài)，在室內(nèi)可使飛行器平穩(wěn)懸停在空中，通過調(diào)節(jié)俯仰角的大小可讓飛行器在室外水平穩(wěn)定向前飛行。

5 結(jié)論

    本文分析了四旋翼飛行器的控制原理，使用牛頓第二定律和歐拉方程建立了系統(tǒng)的數(shù)學(xué)模型，設(shè)計了以STM32高速單片機(jī)為核心的控制系統(tǒng)電路，通過卡爾曼濾波濾去了可能的干擾信號，保證了角度數(shù)據(jù)的精確性。設(shè)計三路串級PID調(diào)節(jié)控制器，實現(xiàn)了對橫滾角、俯仰角、偏航角3種姿態(tài)角控制。室內(nèi)外飛行測試結(jié)果表明，系統(tǒng)可以穩(wěn)定平衡飛行。

參考文獻(xiàn)

[1] 王貞琪，馬潔.四旋翼幾種控制方法比較[J].北京信息科技大學(xué)學(xué)報，2016，31(2)：35-39.

[2] 趙世榮，孫運(yùn)強(qiáng)，石喜玲.基于信息融合四旋翼飛行器穩(wěn)定性改進(jìn)設(shè)計[J].電子器件，2015，38(6)：1417-1420.

[3] 郝偉，鮮斌.四旋翼無人機(jī)姿態(tài)系統(tǒng)的非線性容錯控制設(shè)計[J].控制理論與應(yīng)用，2015，32(11)：1457-1463.

[4] SCHREIER M，DARMSTADT T.Modeling and adaptive control of a quadrotor[C].Proceedings of 2012 IEEE International Conference on Mechatronics and Automation，IEEE，2012：383-390.

[5] 王偉，邱啟明.六旋翼飛行器建模及位置跟蹤控制[J].電子器件，2014，37(3)：507-512.

[6] 廖衛(wèi)中，宗群，馬亞麗.小型四旋翼無人機(jī)建模與有限時間控制[J].控制理論與應(yīng)用，2015，32(10)：1343-1350.

[7] 陳航科，張東升，盛曉超.四旋翼飛行器懸停狀態(tài)姿態(tài)控制建模與仿真[J].計算機(jī)仿真，2013，30(11)：41-45.

[8] 黃建，胡越黎，楊文榮，等.組合導(dǎo)航系統(tǒng)在四旋翼無人機(jī)上的實現(xiàn)[J].電子技術(shù)應(yīng)用，2015，41(5)：167-170.

[9] 葉锃鋒，馮恩信.基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法[J].傳感技術(shù)學(xué)報，2012，25(4)：524-528.

[10] 吳濤，白茹，朱禮堯，等.基于卡爾曼濾波的航姿參考系統(tǒng)設(shè)計[J].傳感技術(shù)學(xué)報，2016，29(4)：531-535.

[11] 何川，李智，王勇軍.基于STM32的四旋翼飛行器的姿態(tài)最優(yōu)估計研究[J].電子技術(shù)應(yīng)用，2015，41(12)：61-64.

[12] 蔡偉平，胡越黎，楊文榮.四旋翼雙環(huán)滑模姿態(tài)控制系統(tǒng)設(shè)計與仿真[J].電子技術(shù)應(yīng)用，2015，41(7)：150-153.

[13] SADEGHZADEH I，MEHTA A，ZHANG Y M，et al.Faulttolerant trajectory tracking control of a quadrotor helicopter using gainscheduled PID and model reference adaptive control[C].Annual Conference of the Prognostics and Health Management Society.Montreal，2011，9：1-10.

[14] 彭程，白越，喬冠宇.四旋翼無人機(jī)的偏航抗飽和與多模式PID控制[J].機(jī)器人，2015，37(4)：415-423.

[15] 甄紅濤，齊曉慧，夏明旗，等.四旋翼無人機(jī)魯棒自適應(yīng)姿態(tài)控制[J].控制工程，2013，20(5)：915-919.