基于STM32的四旋翼自主飛行器控制系統(tǒng)設(shè)計與實現(xiàn)

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

摘要：針對現(xiàn)階段四旋翼飛行器不能自主飛行的問題，提出了一種基于STM32的四旋翼自主飛行器控制系統(tǒng)，該系統(tǒng)通過單片機與MPU6050通信采集陀螺儀、加速度計數(shù)據(jù)，經(jīng)卡爾曼濾波后由方向余弦矩陣解算出姿態(tài)，經(jīng)PID調(diào)節(jié)器控制電機轉(zhuǎn)速實現(xiàn)自主飛行功能。測試表明，該系統(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é)器

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

1 背景

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

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

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

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

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

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

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

3.1硬件總電路圖

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

3.2無刷電機驅(qū)動電路

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

3.3 MPU6050模塊

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

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

3.4 GPS導(dǎo)航電路

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

3.5超聲波定高、測距電路

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

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

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

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

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

4.2超聲波定高及避障

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

5實驗結(jié)果與分析

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

5.1直線飛行測試

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

5.2 矩形飛行測試

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

6 結(jié)束語

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

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

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

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

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

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

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

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