基于雙處理器的四旋翼飛行控制系統(tǒng)研究

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

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

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

DOIDOI：10.11907/rjdk.162226

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

引言

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

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

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

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

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

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

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

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

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

3硬件電路設(shè)計(jì)

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

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

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

4軟件工作流程

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

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

6結(jié)語(yǔ)

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

參考文獻(xiàn)：

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

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

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

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

[5]白立群，李成鐵，周劍峰.基于 STM32 的飛行控制器系統(tǒng)設(shè)計(jì)[J].計(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ì)算機(jī)應(yīng)用與軟件，2012，29（6）：212215.

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

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

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