李艷濤+楊旭+肖銀燕+魯?shù)?宋好舒

摘要：針對(duì)現(xiàn)階段四旋翼飛行器不能自主飛行的問(wèn)題，提出了一種基于STM32的四旋翼自主飛行器控制系統(tǒng)，該系統(tǒng)通過(guò)單片機(jī)與MPU6050通信采集陀螺儀、加速度計(jì)數(shù)據(jù)，經(jīng)卡爾曼濾波后由方向余弦矩陣解算出姿態(tài)，經(jīng)PID調(diào)節(jié)器控制電機(jī)轉(zhuǎn)速實(shí)現(xiàn)自主飛行功能。測(cè)試表明，該系統(tǒng)能夠自主飛行且飛行高度在0～100m以內(nèi)，起飛轉(zhuǎn)速為50 r/s左右，最大巡航速度為11.2 m/s，續(xù)航能力為20 min。

關(guān)鍵詞：四旋翼飛行器；STM32F103VCT6；MPU6050；卡爾曼濾波器；方向余弦矩陣；PID調(diào)節(jié)器

中圖分類號(hào)：TP303 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1009-3044（2016）08-0212-03

1 背景

近年來(lái)，隨著科技的發(fā)展，四旋翼飛行器在人們的生活當(dāng)中扮演者越來(lái)越重的角色，可執(zhí)行水災(zāi)、火災(zāi)、地震等災(zāi)情調(diào)查救援任務(wù)；化工廠等場(chǎng)所有毒氣體濃度監(jiān)測(cè)；重要設(shè)施連續(xù)監(jiān)控；輸油管線和輸電線路的巡查；農(nóng)田、林區(qū)農(nóng)藥噴灑；自然風(fēng)景的取景拍照；當(dāng)對(duì)特定地區(qū)進(jìn)行日常環(huán)境監(jiān)測(cè)，也可以使用這種飛行器，自動(dòng)巡查完后自動(dòng)返航并自動(dòng)記錄存儲(chǔ)數(shù)據(jù)，大大減少人力成本。

在過(guò)去的幾十年里，四旋翼飛行器相對(duì)固定翼飛行器發(fā)展卻較為緩慢，這是因?yàn)樗男盹w行器的控制較固定翼復(fù)雜，早期的技術(shù)水平無(wú)法實(shí)現(xiàn)飛行器的自主飛行控制。為了實(shí)現(xiàn)飛行器的自主飛行，文獻(xiàn)[1]提出了一種雙增益的PD控制算法對(duì)飛行器進(jìn)行姿態(tài)控制； 將姿態(tài)估計(jì)算法和控制算法應(yīng)用到飛行器中， 可以實(shí)現(xiàn)四旋翼的自主懸停等功能。但這種方法不能使飛行器自主的完成一些任務(wù)。文獻(xiàn)[2]提出了一種新的消失點(diǎn)估計(jì)算法—VQME 算法，該算法的正確性、魯棒性與實(shí)時(shí)性得到了驗(yàn)證，并且驗(yàn)證了其在工程應(yīng)用中的可行性。同時(shí)針對(duì)四旋翼飛行器的欠驅(qū)動(dòng)、多耦合等特性，提出了一種級(jí)聯(lián)的多變量RBF神經(jīng)網(wǎng)絡(luò)PID自適應(yīng)控制方法，運(yùn)用該控制方法和以消失點(diǎn)為目標(biāo)點(diǎn)的導(dǎo)航策略，但這種方法無(wú)法實(shí)現(xiàn)大范圍飛行。文獻(xiàn)[3]提出了一種基于磁傳感器的自主導(dǎo)航系統(tǒng)，該系統(tǒng)硬件平臺(tái)采MPU6000（集成了3軸陀螺儀和3軸加速度計(jì)）傳感器實(shí)現(xiàn)姿態(tài)和速度解算，在此基礎(chǔ)上設(shè)計(jì)了基于磁傳感器的航向判斷自主導(dǎo)航算法，通過(guò)實(shí)際飛行器的飛行方向與設(shè)定方向的偏差對(duì)飛行器的姿態(tài)角進(jìn)行調(diào)節(jié)，但這種方法對(duì)于飛行途中的障礙物卻無(wú)法規(guī)避。

綜上所述，國(guó)內(nèi)外在四軸飛行器的研究上得到了極大的發(fā)展，但有一些問(wèn)題急需解決：第一，如何實(shí)現(xiàn)大范圍自主飛行作業(yè)；第二，如何規(guī)避飛行器飛行途中的障礙物。針對(duì)這兩個(gè)問(wèn)題，受文[4-10]的啟發(fā)，本文設(shè)計(jì)了一種利用GPS導(dǎo)航技術(shù)和超聲波測(cè)距技術(shù)來(lái)實(shí)現(xiàn)自主飛行的方法，通過(guò)GPS導(dǎo)航來(lái)指引四旋翼飛行器的飛行路線，通過(guò)超聲波測(cè)距可以實(shí)時(shí)測(cè)量周圍障礙物距四旋翼飛行器的距離并使飛行器繞道飛行，從而實(shí)現(xiàn)飛行器的自主飛行。

2 系統(tǒng)基本工作原理

該系統(tǒng)工作原理框圖如圖1所示， 該系統(tǒng)由STM32F103單片機(jī)、骨架、MPU6050、GPS模塊、超聲波模塊、壓力計(jì)、地磁計(jì)、電池等構(gòu)

成，其中骨架包括電機(jī)、槳翼、電調(diào)等組成，系統(tǒng)本著一體化的設(shè)計(jì)思路，將STM32F103、MPU6050、壓力計(jì)、地磁計(jì)集成在一塊3*5cm的控制板上，使用IIC總線的方式與控制系統(tǒng)連接。首先由GPS模塊獲取當(dāng)前位置狀態(tài)；然后通過(guò)MPU6050上的三軸陀螺儀及三軸加速器獲取四旋翼飛行器的姿態(tài)角并調(diào)節(jié)飛行姿態(tài)；最后由GPS模塊和超聲波模塊指引飛行器進(jìn)行自主飛行。

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

3.1硬件總電路圖

系統(tǒng)硬件部分由核心控制模塊、動(dòng)力模塊、MPU6050模塊、GPS導(dǎo)航模塊、超聲波模塊等組成。其中核心控制模塊由STM32F103芯片和外圍電路組成，外圍電路包括晶振電路、復(fù)位電路和電源電路。硬件總電路圖如圖2所示。

3.2無(wú)刷電機(jī)驅(qū)動(dòng)電路

本設(shè)計(jì)采用四個(gè)電調(diào)驅(qū)動(dòng)四個(gè)920KV自鎖無(wú)刷電機(jī)。7腳和5腳為電機(jī)轉(zhuǎn)向的控制引腳，當(dāng)5腳為高電平，7腳為低電平時(shí)，電機(jī)將順時(shí)針旋轉(zhuǎn)，當(dāng)5腳為低電平，7腳為高電平時(shí)，電機(jī)將逆時(shí)針旋轉(zhuǎn)，當(dāng)5腳和7腳的電平相同時(shí)，電機(jī)將不轉(zhuǎn)動(dòng)，6腳為使能端，可以通過(guò)控制輸出到6腳的PWM信號(hào)的占空比來(lái)控制電機(jī)轉(zhuǎn)速的大小，當(dāng)PWM信號(hào)的占空比為0%時(shí)，電機(jī)將停止轉(zhuǎn)動(dòng)，當(dāng)PWM信號(hào)的占空比為100%時(shí)，電機(jī)轉(zhuǎn)速將達(dá)到最大[6-7]。

3.3 MPU6050模塊

本設(shè)計(jì)采用MPU6050模塊，它集成了3 軸MEMS 陀螺儀，3 軸MEMS加速度計(jì)，以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)算處理器DMP，用IIC接口連接一個(gè)第三方的數(shù)字傳感器。擴(kuò)展之后就可以通過(guò)IIC 接口將飛行器的姿態(tài)角輸出到控制核心模塊。

MPU-6050 對(duì)陀螺儀和加速度計(jì)分別用了三個(gè)16 位的ADC，將其測(cè)量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量?？删_跟蹤快速和慢速的運(yùn)動(dòng)，傳感器的測(cè)量范圍是可控的，陀螺儀可測(cè)范圍為±250，±500，±1000，±2000°/秒（dps），加速度計(jì)可測(cè)范圍為±2，±4，±8，±16g。

3.4 GPS導(dǎo)航電路

GPS模塊集成了RF射頻芯片、基帶芯片和核心CPU，并加上相關(guān)外圍電路而組成的一個(gè)集成電路，GPS模組整合靈敏度高，功耗低，GPS芯片組解決方案在緊湊的設(shè)計(jì)里?？蓪⑼瑫r(shí)追蹤多達(dá)20顆衛(wèi)星，并迅速確定1 Hz導(dǎo)航更新。

3.5超聲波定高、測(cè)距電路

超聲波是頻率高于20000赫茲的聲波，它方向性好，穿透能力強(qiáng)，易于獲得較集中的聲能，超聲波測(cè)距的原理是利用超聲波在空氣中的傳播速度為已知，測(cè)量聲波在發(fā)射后遇到障礙物反射回來(lái)的時(shí)間，根據(jù)發(fā)射和接收的時(shí)間差計(jì)算出發(fā)射點(diǎn)到障礙物的實(shí)際距離，當(dāng)測(cè)得距離小于10cm時(shí)，主程序?qū)?zhí)行繞道飛行的命令。由單片機(jī)產(chǎn)生40KHz 的方波，直接驅(qū)動(dòng)CD4049 芯片，下級(jí)的CD4049 則對(duì)40KHz 頻率信號(hào)進(jìn)行調(diào)理，使超聲波傳感器產(chǎn)生諧振。

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

主程序流程圖如圖3所示，系統(tǒng)首先配置好STM32F103單片機(jī)、GPS導(dǎo)航模塊、超聲波模塊、陀螺儀、加速度計(jì)等硬件設(shè)備；然后，通過(guò)遙控模塊選擇飛行器飛行模式：定高飛行或者GPS飛行；輸入預(yù)設(shè)目的地，通過(guò)傳感器獲得當(dāng)前狀態(tài)參數(shù)，經(jīng)控制系統(tǒng)執(zhí)行算法、處理信息后，飛行器起飛進(jìn)入預(yù)定飛行航道；最后通過(guò)時(shí)刻采集GPS導(dǎo)航模塊、超聲波模塊的數(shù)據(jù)調(diào)節(jié)飛行姿態(tài)，指引飛行器到達(dá)目的地，飛行器到達(dá)目的地后主程序?qū)⒎祷氐较到y(tǒng)上電及初始化，等待下一個(gè)任務(wù)的下達(dá)。

4.1 GPS定位及導(dǎo)航

當(dāng)主程序運(yùn)行到GPS模塊子程序時(shí)，系統(tǒng)首先關(guān)閉中斷，對(duì)GPS模塊進(jìn)行初始化；然后打開(kāi)中斷，允許接收GPS信號(hào)，接著采集GPS數(shù)據(jù)并判斷接收到的數(shù)據(jù)是否為推薦模式，然后再將所接收到的二進(jìn)制碼數(shù)據(jù)通過(guò)串口讀入主控芯片；最后將單片射頻收發(fā)芯片的通道0設(shè)置為飛行數(shù)據(jù)通道，通道1設(shè)置為接收數(shù)據(jù)通道，可實(shí)現(xiàn)單片射頻收發(fā)芯片“多發(fā)單收”的功能，即多路設(shè)備發(fā)送數(shù)據(jù)，單路設(shè)備接收數(shù)據(jù)，可以使程序的運(yùn)行效率更高及數(shù)據(jù)更新速度更快。GPS定位及導(dǎo)航流程圖如圖4所示。

4.2超聲波定高及避障

飛行器的定高及避障是靠超聲波對(duì)周圍障礙物、地面的探測(cè)來(lái)實(shí)現(xiàn)的。因此，本部分的程序設(shè)計(jì)主要是針對(duì)超聲波模塊。首先對(duì)超聲波模塊初始化，選擇飛行模式：定高飛行、GPS飛行。然后由單片機(jī)向模塊的Trig管腳輸入一個(gè)10us的高電平，模塊檢測(cè)到這個(gè)10us的高電平信號(hào)后就會(huì)發(fā)出8個(gè)40KHZ的超聲波脈沖信號(hào)，此時(shí)打開(kāi)定時(shí)器中斷開(kāi)始計(jì)時(shí)，當(dāng)檢測(cè)到回波信號(hào)后，停止定時(shí)器中斷，單片機(jī)對(duì)數(shù)據(jù)進(jìn)行處理得到此刻探測(cè)到的距離。最后當(dāng)選擇定高飛行模式時(shí)，若檢測(cè)到距地面的距離到達(dá)設(shè)定數(shù)值時(shí)，保持飛行姿態(tài)；當(dāng)選擇GPS飛行模式時(shí)，若檢測(cè)到距前方物體在1m之內(nèi)，飛行器將改變飛行方向。超聲波測(cè)距模塊流程圖如圖5所示。

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

實(shí)地飛行測(cè)試是檢測(cè)對(duì)控制系統(tǒng)的功能和技術(shù)指標(biāo)的最終手段，也是衡量四軸設(shè)計(jì)是否成功的重要標(biāo)志。當(dāng)前面的各項(xiàng)調(diào)試都順利完成后，就要準(zhǔn)備進(jìn)行試飛實(shí)驗(yàn)。試飛前要確保系統(tǒng)各部分工作正常且穩(wěn)定。確保各個(gè)接口連接正確，確保各部件安裝牢固，確保電池電量充足。一切準(zhǔn)備就緒，即可按以下步驟進(jìn)行試飛。

5.1直線飛行測(cè)試

一，將四軸飛行器放在水平地面上，打開(kāi)電源開(kāi)關(guān)；二，將飛行高度設(shè)定在1.5m；三，將飛行距離設(shè)置為50m；四，按下按鍵，一鍵式起飛完成飛行任務(wù)。由實(shí)際情況可看出該設(shè)計(jì)實(shí)現(xiàn)了平穩(wěn)起飛，定高等功能。在微風(fēng)的干擾下機(jī)體晃動(dòng)能夠自動(dòng)調(diào)整姿態(tài)，確保平穩(wěn)飛行，且系統(tǒng)響應(yīng)快。

5.2 矩形飛行測(cè)試

一，將四軸飛行器放在水平地面上，打開(kāi)電源開(kāi)關(guān)；二，將飛行高度設(shè)定在80cm；三，將飛行軌跡設(shè)定為延逆時(shí)針邊長(zhǎng)為5m的矩形；四，按下按鍵，一鍵式起飛完成飛行任務(wù)。由實(shí)際情況可看出該設(shè)計(jì)實(shí)現(xiàn)了循跡飛行，定高等功能。在微風(fēng)的干擾下機(jī)體晃動(dòng)能夠自動(dòng)調(diào)整姿態(tài)，確保平穩(wěn)飛行，且系統(tǒng)響應(yīng)快。

6 結(jié)束語(yǔ)

與以往的四旋翼飛行器相比，本文設(shè)計(jì)的四旋翼飛行器最大的優(yōu)點(diǎn)是能夠?qū)崿F(xiàn)自主飛行，其飛行高度在0-100m以內(nèi)，并利用超聲波模塊可在3m內(nèi)定高，定高可精確到厘米級(jí)。由于現(xiàn)階段能力及資源有限，本設(shè)計(jì)還有很多需要改進(jìn)的地方，例如：可以添加電子羅盤(pán)，將偏航角引入到導(dǎo)航計(jì)算中，從而使飛行器的飛行路線更加精確與穩(wěn)定；同時(shí)可以加入無(wú)線模塊，使此系統(tǒng)分為機(jī)載控制與地面控制兩塊，便于數(shù)據(jù)的采集與飛行器的實(shí)時(shí)監(jiān)控。

參考文獻(xiàn)：

[1] WANG Shao-hua ， YANG Ying. Quadrotor aircraft attitude estimation and control based on Kalman filter[J]. Control Theory & Applications， 2013， 30（9）：1109-1115.

[2] Seong Hwang， Seung Yong Min， Choong Hee Lee， Seung Jo KiM. Development Of A Four-rotor Cyclocopter[J]. Journal of Aircraft， 2008， 45（6）：1-6.

[3] 張帆， 曹喜濱， 鄒經(jīng)湘. 一種新的全角度四元數(shù)與歐拉角的轉(zhuǎn)換算法[J]. 南京理工大學(xué)學(xué)報(bào)， 2002， 26（4）： 3-8.

[4] 王學(xué)斌， 徐建宏， 張章. 卡爾曼濾波器參數(shù)分析與應(yīng)用方法研究[J]. 北京： 北京北方計(jì)算中心， 2012， 29（6）： 212-215.

[5] 施文明， 徐彬， 陳利敏. 捷聯(lián)式航姿系統(tǒng)中四元數(shù)算法Kalman濾波器的實(shí)現(xiàn)研究[J]. 自動(dòng)化技術(shù)應(yīng)用， 2005， 24（11）： 6-12.

[6] 李俊， 李運(yùn)堂. 四旋翼飛行器的動(dòng)力學(xué)建模及PID控制[J]. 遼寧工程技術(shù)大學(xué)學(xué)報(bào)， 2012， 31（1）： 4-7.

[7] Michael McRoberts. Arduino從基礎(chǔ)到實(shí)踐[M]. 楊繼志， 郭敬，譯.北京：電子工業(yè)出版社， 2013： 1-447.

[8] 程晨. Arduino開(kāi)發(fā)實(shí)戰(zhàn)指南：AVR篇[M]. 北京： 機(jī)械工業(yè)出版社， 2012： 1-321.

[9] 劉火良， 楊森.STM32庫(kù)開(kāi)發(fā)實(shí)戰(zhàn)指南[M].北京： 機(jī)械工業(yè)出版社，2013：1-496.

[10] 胡琴， 施雅， 陳嘉聰，等. 四旋翼自主飛行器系統(tǒng)[J]. 科技致富向?qū)В?2013， 35（24）：235-237.