吳思穎+李亞楠+王年豐+姚遠(yuǎn)

摘要摘要：為提高四旋翼飛行器工作可靠性，采用STM32F407VGT和R5F100LE處理器，設(shè)計了四旋翼飛行器控制系統(tǒng)。加速度陀螺儀MPU6050模塊采集飛行姿態(tài)數(shù)據(jù)，超聲測距模塊、激光傳感模塊和攝像頭模塊提供導(dǎo)航參數(shù)，采用濾波算法對傳感器所采集的數(shù)據(jù)進(jìn)行處理，使用PID控制算法實現(xiàn)尋線前進(jìn)。實驗測試表明，飛行器能一鍵起飛，完成空投任務(wù)，精確降落。

關(guān)鍵詞關(guān)鍵詞：四旋翼飛行器；姿態(tài)控制；雙處理器；濾波算法

DOIDOI：10.11907/rjdk.162226

中圖分類號：TP319文獻(xiàn)標(biāo)識碼：A文章編號文章編號：16727800（2017）001004403

引言

四旋翼飛行器是一個多傳感器以及眾多子系統(tǒng)（導(dǎo)航、通信、飛行控制等）構(gòu)成的高集成控制系統(tǒng)。近年來，隨著嵌入式處理器、傳感技術(shù)、控制技術(shù)的發(fā)展，尤其是智能控制算法的應(yīng)用，其性能得到了極大提高，在測控、巡檢等軍事和民用領(lǐng)域得到了廣泛應(yīng)用[12]，具有極高的研究價值和應(yīng)用潛力。

四旋翼飛行器有4個輸入量、6個自由度輸出，調(diào)整飛行姿態(tài)和實現(xiàn)室內(nèi)定位是四旋翼飛行器設(shè)計的難點(diǎn)之一[3]。現(xiàn)有的飛行控制系統(tǒng)一般采用ARM7、DSP等高速處理器作為控制芯片。這類單芯片飛控系統(tǒng)，在一個控制周期內(nèi)要完成數(shù)據(jù)采集、數(shù)據(jù)處理、控制運(yùn)算及指令輸出，還需將數(shù)據(jù)輸出到監(jiān)控系統(tǒng)，實現(xiàn)室內(nèi)定位和尋跡飛行，會產(chǎn)生過重負(fù)荷，導(dǎo)致系統(tǒng)可靠性下降。為提高系統(tǒng)可靠性，本文采用ST公司的STM32F407VGT負(fù)責(zé)主控，瑞薩單片機(jī)R5F100LE處理器負(fù)責(zé)數(shù)據(jù)處理，調(diào)制PWM（Pulse Width Modulation）信號輸出占空比驅(qū)動無刷直流電機(jī)，姿態(tài)傳感器（MPU6050集成模塊）檢測飛行器的加速度和角速度，攝像頭尋跡模塊檢測地面導(dǎo)航線，US-100超聲測距模塊獲取飛行器的飛行高度，運(yùn)用卡爾曼濾波算法對傳感器數(shù)據(jù)進(jìn)行分析融合，串級PID算法對飛行器進(jìn)行控制，實現(xiàn)四旋翼飛行器的雙處理器飛行控制系統(tǒng)設(shè)計。

1飛行控制系統(tǒng)

本設(shè)計采用分工協(xié)作機(jī)制，由ST公司的STM32F407VGT負(fù)責(zé)主控，瑞薩單片機(jī)R5F100LE負(fù)責(zé)數(shù)據(jù)處理以提高控制可靠性。其中STM32F407VGT采用先進(jìn)的Coretex-M4內(nèi)核，最高處理速度可達(dá)210DMIPS的處理能力，存儲空間大，具有浮點(diǎn)運(yùn)算能力和增強(qiáng)處理指令的優(yōu)點(diǎn)，帶有多種外設(shè)接口，如照相機(jī)接口、加密處理器和帶FIFO的DMA控制器等。而R5F100LE單片機(jī)內(nèi)置高速振蕩器時鐘，最高頻率可達(dá)32MHZ，其最短指令執(zhí)行時間可在高速至超低速間更改，自帶單電源閃存，支持自編程功能，內(nèi)置上電復(fù)位、看門狗定時器、按鍵中斷、時鐘輸出/蜂鳴器輸出控制電路等功能，使用極其方便。

四旋翼飛行器的4個動力臂呈十字交叉狀固定在四旋翼飛行器中心部件上，每個動力臂末端的電機(jī)座上固定一個電機(jī)和螺旋槳。通過改變電機(jī)轉(zhuǎn)速，可使飛行器在其慣性參考坐標(biāo)系中產(chǎn)生六自由度運(yùn)動，包括3個坐標(biāo)軸方向的線運(yùn)動（進(jìn)退、左右側(cè)飛和升降）和3個坐標(biāo)軸方向的角運(yùn)動（偏航、俯仰和橫滾），可實現(xiàn)在有限區(qū)域的垂直起降、穩(wěn)定盤旋以及精確目標(biāo)移動等飛行姿態(tài)控制[1]。

姿態(tài)控制是整個飛行控制的基礎(chǔ)。姿態(tài)控制系統(tǒng)通過檢測飛行器在機(jī)體坐標(biāo)系下3個軸向的角速度、角度和相對地面的高度等相應(yīng)姿態(tài)信息，采用相應(yīng)的控制算法解算出4個電機(jī)的轉(zhuǎn)速，發(fā)送給電機(jī)調(diào)速器調(diào)整電機(jī)轉(zhuǎn)速，實現(xiàn)對飛行器飛行姿態(tài)的調(diào)整。

2相關(guān)控制方法

四旋翼飛行器姿態(tài)調(diào)整的控制算法很多，如DI（digital input）控制、PID（比例微分積分）控制以及LQ控制等。其中，PID控制算法具有適用面廣、控制參數(shù)相互獨(dú)立、參數(shù)選定相對簡單等優(yōu)點(diǎn)[45]，本系統(tǒng)采用PID控制算法實現(xiàn)飛行姿態(tài)控制。該算法可以分別計算出飛行器的自轉(zhuǎn)調(diào)節(jié)量、左右傾斜調(diào)節(jié)量、油門控制調(diào)節(jié)量、前后俯仰調(diào)節(jié)量等，使系統(tǒng)穩(wěn)定偏差最小，保證飛行器能按照動作指令有效完成指定動作，如尋線和定高飛行等。PID控制系統(tǒng)原理：①比例環(huán)節(jié)可以準(zhǔn)實時成比例地反映控制系統(tǒng)的偏差信號變化；②積分環(huán)節(jié)主要用于消除靜差，提高系統(tǒng)的無差度；③微分環(huán)節(jié)反應(yīng)偏差信號的變化趨勢（變化速率），加快系統(tǒng)的動作速度，減小調(diào)節(jié)時間，如圖1所示。

理[67]，計算出當(dāng)前的飛行姿態(tài)和地面控制需要的目標(biāo)姿態(tài)，比較實際姿態(tài)和目標(biāo)姿態(tài)的差異，估計過程狀態(tài)，并使估計均方差最?。焕肞ID算法計算出每個電機(jī)的調(diào)整量，調(diào)整電機(jī)轉(zhuǎn)速，從而實現(xiàn)飛行姿態(tài)的調(diào)整。為了得到穩(wěn)定、準(zhǔn)實時的飛行姿態(tài)，以定高飛行為例，利用超聲波傳回的高度值作為外環(huán)，將加速度計及加速度計積分得出的Z軸方向速度值作為內(nèi)環(huán)，外環(huán)的輸出量解算后成為內(nèi)環(huán)的期望值，內(nèi)環(huán)通過PID控制器將計算得出的量疊加在油門上控制高度，通過調(diào)節(jié)PID參數(shù)值，用先整定內(nèi)環(huán)后整定外環(huán)的方法確定PID參數(shù)，完成穩(wěn)定的定高控制。在實際飛行中需調(diào)節(jié)KP、TI、TD和t這4個常數(shù)，這是采用PID算法進(jìn)行控制的重點(diǎn)和難點(diǎn)。本系統(tǒng)借助實驗對這4個常數(shù)進(jìn)行調(diào)節(jié)，得到一組較可靠的飛行姿態(tài)控制數(shù)據(jù)。借助視頻傳感器（攝像頭）采集圖像，通過對圖像的二值化處理，統(tǒng)計每一行和每一列的灰度值，算出灰度直方圖，根據(jù)行（列）的灰度總值變化趨勢，分析灰度出現(xiàn)突變的行列位置，得出導(dǎo)航線橫向的縱坐標(biāo)，豎線的橫坐標(biāo)，然后根據(jù)坐標(biāo)進(jìn)行飛行器的循線導(dǎo)航控制。

3硬件電路設(shè)計

系統(tǒng)硬件分為6個部分[67]，如圖2所示。超聲傳感器負(fù)責(zé)測量高度，和陀螺儀模塊一起實現(xiàn)定高控制；視頻傳感器（攝像頭）負(fù)責(zé)尋跡控制；激光傳感器完成電子視高控制；電機(jī)模塊負(fù)責(zé)按照主控信號驅(qū)動電機(jī)；姿態(tài)傳感部分負(fù)責(zé)獲取飛行姿態(tài)。為了提高處理速度，激光傳感器和視頻傳感器傳回的數(shù)據(jù)先傳入瑞薩單片機(jī)（R5F100LE）計算處理，處理結(jié)果作為引導(dǎo)量輸出到主控，再由主控使用PID算法完成穩(wěn)定的尋跡前進(jìn)，并實現(xiàn)物體的拾取和定點(diǎn)空投。

考慮到超聲波傳感器具有方向性好、能定向傳播的優(yōu)點(diǎn)，超聲測距模塊US100被用來測量四旋翼飛行器的飛行高度。該模塊使用簡單、測量準(zhǔn)確，而且能使用串口指令讀取數(shù)據(jù)[810]，與陀螺儀模塊結(jié)合可校正高度數(shù)據(jù)，實現(xiàn)定高飛行控制。為實現(xiàn)姿態(tài)獲取和控制，本系統(tǒng)集成了三軸加速度計和三軸陀螺儀MPU6050模塊實現(xiàn)飛行姿態(tài)感知，使用卡爾曼濾波處理數(shù)據(jù)后輸出比較穩(wěn)定的姿態(tài)角，輸出加速度和陀螺儀的值。數(shù)字運(yùn)動處理（DMP：Digital Motion Processing）引擎可減少復(fù)雜的融合演算數(shù)據(jù)，較容易地獲取和矯正飛行姿態(tài)；MPU6050模塊集成的陀螺儀測量范圍為±500dps，加速度計測量范圍為±2g。對陀螺儀和加速度計分別使用3個16位的ADC進(jìn)行模數(shù)變化，采集速度快，能以數(shù)字輸出6軸或9軸融合演算數(shù)據(jù)并可程式控制，消除加速器與陀螺儀軸間敏感度，降低設(shè)定的影響度及感測器飄移。

由于直流電機(jī)工作電流大，硬件電路設(shè)計時需注意MCU與直流電機(jī)的隔離，若隔離措施不到位，容易燒壞MCU的端口。由于主控要連接較多的外圍電路，為了減少反接意外情況，將所有供電系統(tǒng)都通過保險絲控制，具體電路如圖3所示。

4軟件工作流程

四旋翼飛行器軟件主要分為3部分：①電機(jī)驅(qū)動部分。利用處理器的多路PWM輸出功能驅(qū)動電機(jī)；②數(shù)據(jù)接收和處理部分。獲取并處理飛行姿態(tài)數(shù)據(jù)；③姿態(tài)控制部分。根據(jù)接收到的傳感器數(shù)據(jù)，運(yùn)用相應(yīng)算法，使飛行器姿態(tài)平衡，實現(xiàn)特定狀態(tài)飛行。主程序根據(jù)要求設(shè)定不同的飛行控制模式，具體工作模式流程如圖4所示。系統(tǒng)軟件工作流程如圖5所示。

為驗證系統(tǒng)的可靠性和穩(wěn)定性，分3種情況進(jìn)行測試：①測試尋跡往返飛行。在飛行高度70cm時，測試降落地與設(shè)定中心的最大偏差為38cm；②測試尋跡定高載物定點(diǎn)投放往返飛行。在飛行高度70-79cm時，測試物體落地與定點(diǎn)投放中心的最大偏差為50cm，降落地與設(shè)定中心的偏差為38cm，在5次測試中碰觸定高線引發(fā)報警1次；③測試尋跡定點(diǎn)拾取物體往返飛行。測試降落地與設(shè)定中心的最大偏差為40cm。測試結(jié)果表明本文設(shè)計的四旋翼飛行器飛行高度穩(wěn)定、姿態(tài)解算正確，能實現(xiàn)尋跡、定高飛行、定點(diǎn)投放、拾取重物等多種需求。

6結(jié)語

本文采用ST公司的STM32F407VGT處理器和瑞薩R5F100LE單片機(jī)，設(shè)計實現(xiàn)了四旋翼飛行器控制系統(tǒng)，可實現(xiàn)定向飛行和定高飛行，在規(guī)定線路內(nèi)的自主飛行，并且能夠通過小型電磁鐵吸取薄鐵片飛至指定區(qū)域完成空投。測試表明飛行器能一鍵起飛，完成空投任務(wù)，精確降落。

參考文獻(xiàn)：

[1]楊云高，鮮斌，殷強(qiáng)，等.四旋翼無人飛行器架構(gòu)及飛行控制的研究現(xiàn)狀[C].第三十屆中國控制會議，中國煙臺， IEEE Press，2010：448453.

[2]黃智偉.全國大學(xué)生電子設(shè)計競賽常用電路模塊制作[M].北京：北京航空航天大學(xué)出版社，2011.

[3]聶博文.微小型四旋翼飛行器的研究現(xiàn)狀與關(guān)鍵技術(shù)[J].電光與控制，2007，14（6）：113117.

[4]楊明志，王敏.四旋翼微型飛行器控制系統(tǒng)設(shè)計[J].計算機(jī)測量與控制，2008，16（4）485490.

[5]白立群，李成鐵，周劍峰.基于 STM32 的飛行控制器系統(tǒng)設(shè)計[J].計算機(jī)應(yīng)用，2013，32（2）：1533.

[6]HOFFMANN F，GODDEMEIER N，BERTRAM T.Attitude estimation and control of a quadrocopter [C].The 2010 IEEE/RSJ International conference on Intelligent Robots and Systems.Taipei：IEEE，2010：10721077 .

[7]王學(xué)斌，徐建宏，張章.卡爾曼濾波器參數(shù)分析與應(yīng)用方法研究[J].計算機(jī)應(yīng)用與軟件，2012，29（6）：212215.

[8]李強(qiáng)，林明耀.無傳感器無刷直流電機(jī)控制系統(tǒng)及其起動分析[J].微電機(jī)，2003，36（3）：3335.

[9]周權(quán)，黃向華，朱理化.四旋翼微型飛行平臺姿態(tài)穩(wěn)定控制試驗研究[J].傳感器與微系統(tǒng)，2009，28（5）：7279.

[10]黃敦華，朱青松.基于微小型四旋翼飛行器的目標(biāo)監(jiān)測與識別綜述[J].機(jī)電產(chǎn)品開發(fā)與創(chuàng)新，2011，24（6）：1618.