鞏夢(mèng)巖，孟 青，程思強(qiáng)，李 夢(mèng)

?

基于STM32的四旋翼自主投靶飛行器

鞏夢(mèng)巖，孟 青，程思強(qiáng)，李 夢(mèng)

（中北大學(xué)，山西 朔州 036000）

本項(xiàng)目在各模塊相互協(xié)作下，四旋翼飛行器可以按照規(guī)定航向以某一高度穩(wěn)定飛行，精確降落，并進(jìn)行拾取，投放鐵片等各項(xiàng)任務(wù)。該飛行器由主控制板、飛行控制板、定點(diǎn)模塊、電磁鐵模塊、超聲波模塊、電機(jī)模塊等部分組成。采用意法半導(dǎo)體集團(tuán)的STM32F103ZET6芯片作為主控制板，飛行控制板MCU為STM32F103RCT6，通過MPU6050（加速度計(jì)，陀螺儀），IST8310（磁力計(jì)），HC-SR04（超聲波模塊）獲得姿態(tài)與高度數(shù)據(jù)進(jìn)行PID運(yùn)算后實(shí)現(xiàn)定高飛行。定點(diǎn)和循跡模塊使用Openmv攝像頭對(duì)黑點(diǎn)位置進(jìn)行采樣，經(jīng)過數(shù)據(jù)處理后控制飛行器可以保持在目標(biāo)黑點(diǎn)正上方從而完成定點(diǎn)和循跡任務(wù)。磁鐵模塊在MCU的控制下可以準(zhǔn)確的完成拾取、投放鐵片的任務(wù)。

STM32單片機(jī)；飛行姿態(tài)；自主懸停；PID算法

0 引言

四旋翼飛行器通過調(diào)節(jié)占空比來調(diào)節(jié)四個(gè)電機(jī)轉(zhuǎn)速，實(shí)現(xiàn)升力的變化，從而控制飛行器進(jìn)行橫滾，俯仰來改變姿態(tài)和位置。近年來，無人機(jī)的研究和應(yīng)用[1]廣泛受到各個(gè)方面的重視。四旋翼飛行器作為無人機(jī)的一種，能夠垂直起降、空中懸停、可適用于各種飛行場景，具有靈活度高，安全性好的特點(diǎn)，適用于警務(wù)監(jiān)控、航拍攝影、火場指揮、交通管理、地質(zhì)災(zāi)害調(diào)查、管線巡航等領(lǐng)域?qū)崿F(xiàn)空中時(shí)時(shí)移動(dòng)監(jiān)控。因此，四旋翼飛行器的研究非常符合當(dāng)今社會(huì)的發(fā)展趨勢(shì)。

1 系統(tǒng)理論分析與計(jì)算

1.1 飛行器的模型建立

飛行器運(yùn)動(dòng)過程可以分解成機(jī)體繞三個(gè)軸運(yùn)動(dòng)來描述：當(dāng)飛行器前后運(yùn)動(dòng)時(shí)，飛行器即做俯仰運(yùn)動(dòng)，飛行器在-平面進(jìn)行繞軸運(yùn)動(dòng)，產(chǎn)生的角度即俯仰角（pitch）；當(dāng)飛行器發(fā)生轉(zhuǎn)向運(yùn)動(dòng)時(shí)，飛行器將產(chǎn)生偏航，飛行器在-平面進(jìn)行繞軸運(yùn)動(dòng)，產(chǎn)生的角度即航向角（yaw）；當(dāng)讓飛行器側(cè)身移動(dòng)時(shí)，飛行器將要做橫滾運(yùn)動(dòng)，飛行器在-平面進(jìn)行繞軸轉(zhuǎn)動(dòng)，產(chǎn)生的角度即橫滾角（roll）。如下圖。

圖1 機(jī)體坐標(biāo)圖

1.2 濾波器的設(shè)計(jì)

針對(duì)傳感器內(nèi)部低通后輸出數(shù)據(jù)具有滯后性，在此飛控中對(duì)傳感器采集時(shí)不設(shè)置內(nèi)部低通，比較原始信號(hào)在四旋翼懸停油門附近與靜止時(shí)的頻譜圖，確定因震動(dòng)產(chǎn)生的傳感器噪聲截止頻率，設(shè)計(jì)二階巴特沃斯濾波器[2]對(duì)噪聲信號(hào)進(jìn)行處理。同時(shí)針對(duì)姿態(tài)解算與慣性導(dǎo)航主導(dǎo)傳感器不同，本項(xiàng)目中同一量在不同使用條件下的截止頻率也不一樣。

1.3 姿態(tài)解算

姿態(tài)解算[3]是四旋翼飛行器飛行的基礎(chǔ)，需要結(jié)合數(shù)據(jù)融合以及數(shù)據(jù)濾波，以及空間旋轉(zhuǎn)描述，從而確定飛行器的絕對(duì)或相對(duì)旋轉(zhuǎn)（姿態(tài)）。利用互補(bǔ)濾波、梯度下降、EKF算法完成四元數(shù)的更新，經(jīng)歐拉角轉(zhuǎn)換得出姿態(tài)的俯仰和橫滾角。此飛控中，避免磁力計(jì)干擾引起水平姿態(tài)角解算錯(cuò)誤，磁力計(jì)不參與水平姿態(tài)矯正，直接采用一階互補(bǔ)濾波算法獲取飛行器的偏航角。

1.4 慣性導(dǎo)航

根據(jù)加速度計(jì)比力模型，將三軸加速度計(jì)原始數(shù)字量，通過在載體系統(tǒng)到導(dǎo)航系下的旋轉(zhuǎn)矩陣歸一化處理后，得到導(dǎo)航坐標(biāo)系下比力加速度，或去重量加速度后，得到用以導(dǎo)航的運(yùn)動(dòng)加速度，本項(xiàng)目前期對(duì)飛控采用的基于經(jīng)典回路反饋法的三階互補(bǔ)方案原理進(jìn)行了論證與設(shè)計(jì)實(shí)現(xiàn)，接著對(duì)豎直位置、速度采用單觀測量的卡爾曼濾波進(jìn)行慣導(dǎo)融合，接著針對(duì)氣壓計(jì)觀測傳感器滯后性，導(dǎo)致快速運(yùn)動(dòng)時(shí)，慣導(dǎo)收斂慢問題，提出了一種帶延時(shí)修正的慣性融合算法。目前水平方向慣導(dǎo)融合采用的是雙觀測量帶延時(shí)修正的卡爾曼濾波慣導(dǎo)算法。

1.5 PID控制

為了達(dá)到較好的飛行器系統(tǒng)控制效果，該項(xiàng)目采用了串級(jí)PID[4-6]。PID的流程通過誤差信號(hào)控制被控量，這里我們規(guī)定（在t時(shí)刻）：

1. 輸入量（期望值）為rin(t)；

2. 當(dāng)前量（實(shí)際值）為rout(t)；

3. 偏差量（期望值減去實(shí)際值）為e(t)=rin(t)-rout(t)；

PID的控制規(guī)律為：

在只有角度單環(huán)的控制下，若遇到外力或磁場干擾下，系統(tǒng)很難穩(wěn)定運(yùn)行。因此，加入角速度作為內(nèi)環(huán)，由陀螺儀采集數(shù)據(jù)輸出的角速度值一般不受外界影響，抗干擾能力強(qiáng)。所以四軸飛行器采用雙閉環(huán)PID控制，角度作為外環(huán)，角速度作為內(nèi)環(huán)，進(jìn)行姿態(tài)PID控制；當(dāng)定高時(shí)，高度作為外環(huán)，軸加速度作為內(nèi)環(huán)，進(jìn)行高度PID控制。其中，PID輸出為油門值，控制電機(jī)轉(zhuǎn)速，使空間三軸歐拉角和高度發(fā)生變化。

1.6 飛行高度控制

采用超聲波測距模塊，測量距離2 cm-120 cm。利用系統(tǒng)板向模塊發(fā)送觸發(fā)模塊，通過測量echo腳返回的信號(hào)的高電平部分時(shí)長計(jì)算測量到的距離。原理圖如圖2所示。

通過Echo端輸出一高電平，可根據(jù)這一高電平的持續(xù)時(shí)間來計(jì)算距離值。即距離值為聲速與高電平時(shí)間之積/2，通過得到的高度后進(jìn)行計(jì)算可以得到油門調(diào)節(jié)值對(duì)初始設(shè)定油門進(jìn)行調(diào)節(jié)，使得電機(jī)的轉(zhuǎn)速所形成的升力恰好可以與重力平衡，這樣就可以使飛機(jī)保持在某一高度。

1.7 飛行路徑控制

飛行路徑的控制[7]主要是根據(jù)Openmv探測到的坐標(biāo)值，計(jì)算路面黑線的中心是否在飛行器的正下方，主控制板進(jìn)行PID運(yùn)算后輸出PWM波經(jīng)過飛控板處理后控制電機(jī)使飛行器進(jìn)行橫滾，使得飛行器沿著黑線方向前進(jìn)。

圖2 超聲波原理圖

1.8 薄鐵片拾取與投放控制

根據(jù)電磁鐵的通電具有磁性，斷電磁性消失的原理，從A起飛時(shí)我們讓單片機(jī)控制電磁鐵通電，讓飛行器吸取薄鐵片飛向B區(qū)，到達(dá)B區(qū)后讓電磁鐵斷電，從而投下薄鐵片，讓其落到B區(qū)。

2 電路與程序設(shè)計(jì)

2.1 系統(tǒng)整體框架設(shè)計(jì)

按鍵，攝像頭模塊，超聲波模塊，電磁鐵模塊均與STM32主控制板通過串口相連接，經(jīng)過主控制板處理數(shù)據(jù)后輸出PWM波由飛控板處理后控制電機(jī)轉(zhuǎn)速來使飛行器做出相應(yīng)的飛行姿態(tài)。

圖3 整體流程圖

2.2 系統(tǒng)整體電路圖

圖4 整體電路圖

2.3 程序設(shè)計(jì)

程序流程圖如下所示：

圖5 程序流程圖

主要軟件設(shè)計(jì)如圖6所示。

圖6 軟件設(shè)計(jì)圖

3 測試方案與測試結(jié)果

3.1 測試方案

（1）硬件測試

檢查飛行器各部分的連接，卸掉飛行器的機(jī)槳，運(yùn)行程序，檢查電調(diào)聲音，感受各個(gè)電機(jī)的轉(zhuǎn)速，測試期間將各部分?jǐn)?shù)據(jù)發(fā)送至地面站，通過觀察數(shù)據(jù)和電路連接檢查電路各硬件部分狀況。

（2）軟件仿真測試

使用ST-Link線上調(diào)試飛控板，RL78G13仿真板線上調(diào)試主控制板，觀察軟件運(yùn)行是否流暢。

（3）硬件軟件聯(lián)調(diào)

將飛行器放在圓形區(qū)域，讓它自主飛行，觀察飛行器的飛行高度與飛行方向和時(shí)間，若飛行器不能按照預(yù)定方案飛行就逐項(xiàng)調(diào)試程序的PID參數(shù)和pitch，roll,throttle,yaw等參數(shù)，再進(jìn)行調(diào)試。

3.2 測試條件與儀器

測試條件：多次檢查，硬件電路必須和設(shè)計(jì)原理圖完全一致。檢查焊點(diǎn)，確保無虛焊。電機(jī)轉(zhuǎn)速平穩(wěn)，轉(zhuǎn)向正常，電調(diào)聲音正常，不會(huì)有突變的情況發(fā)生。數(shù)據(jù)收發(fā)正常，無數(shù)據(jù)亂跳的情況。上槳后，檢查槳的正反，保證安裝正確。上槳飛行必須在安全防護(hù)網(wǎng)里。

測試儀器：學(xué)生電源，示波器，數(shù)字式萬用表。

3.3 測試結(jié)果及分析

在早期的測量過程中，硬件軟件都出現(xiàn)過不少問題，很多時(shí)候飛行器會(huì)出現(xiàn)向一個(gè)方向偏或自旋，而且遙控器需要很大的調(diào)節(jié)力度才能調(diào)節(jié)過來，有的時(shí)候甚至直接炸機(jī)。這些情況的出現(xiàn)，在硬件上很多情況下是飛行器的重心不在中心；或是在某次撞機(jī)或長時(shí)間實(shí)驗(yàn)后，電機(jī)電調(diào)等偏離正常參數(shù)性能；傳感器接口接觸不良導(dǎo)致數(shù)據(jù)混亂。軟件上，很多情況下是參數(shù)沒有調(diào)好；或者是中斷程序的相互沖突，數(shù)據(jù)處理周期過大等。當(dāng)然還有編程算法方面的錯(cuò)誤。在多次測量后，最后幾次穩(wěn)定的數(shù)據(jù)如表1所示。

表1 測試結(jié)果表

Tab.1 Test results table

4 結(jié)束語

該四旋翼飛行系統(tǒng)以單片機(jī)控制為核心，能夠?qū)崟r(shí)的采集飛行器當(dāng)前的姿態(tài)信息并融合解算出當(dāng)前的歐拉角。采用先進(jìn)的串級(jí)PID控制方法同時(shí)結(jié)合超聲波以及攝像頭模塊反饋回來的信息對(duì)當(dāng)前姿態(tài)角進(jìn)行控制，從而完成俯仰，橫滾、偏航等各種動(dòng)作以實(shí)現(xiàn)按照規(guī)定航路自主飛行，并通過電磁鐵吸引鐵片完成投靶任務(wù)。

[1] 宋洪達(dá), 郝桂麗. 四旋翼飛行器的發(fā)展與應(yīng)用[J]. 科技風(fēng), 2018, 31(1): 134.

[2] 王賽男, 邢冬梅. 基于MATLAB 的小波閾值濾波與應(yīng)用[J]. 軟件, 2015, 36(10): 102-104.

[3] 曹延超. 基于STM32 的四旋翼飛行器姿態(tài)測量系統(tǒng)設(shè)計(jì)[J]. 軟件, 2015, 36(1): 104-109.

[4] 靳亞磊, 李虹, 李昕濤. 四旋翼飛行器串級(jí)PID控制設(shè)計(jì)與實(shí)現(xiàn)[J]. 太原科技大學(xué)學(xué)報(bào), 2018, 39(3): 177-183.

[5] 范云飛, 任小洪, 袁文林. 基于并聯(lián)PID 的四旋翼飛行控制策略設(shè)計(jì)[J]. 軟件, 2015, 36(4): 37-39.

[6] 王貞琪, 馬潔. 四旋翼幾種控制方法比較[J]. 北京信息科技大學(xué)學(xué)報(bào)(自然科學(xué)版), 2016, 31(2): 35-39.

[7] 王忠生, 閆超. 四旋翼飛行器的懸?？刂茊栴}探討[J]. 赤峰學(xué)院學(xué)報(bào)(自然科學(xué)版), 2017, 38(8): 12-13.

Four Rotor Autonomous Target Vehicle Based on STM32

GONG Meng-yan, MENG Qing, CHENG Si-qiang, LI Meng

(North University of China, Shuozhou 03600)

This project under each module mutual cooperation, the Four-rotor aircraft may according to the stipulation heading fixed flight, precise landing, and picks up, throws the iron sheet and so on each task. The aircraft is composed of main control board, row control board, fixed-point module, electromagnet module, ultrasonic module, motor module and so on. The STM32F103ZET6 chip of STMicroelectronics is used as the main control board, the flight control board MCU is STM32F103RCT6, and the attitude and altitude data are obtained by MPU6050 (accelerometer, gyroscope), IST8310 (magnetometer), HC-SRO4 (ultrasonic module) to realize the fixed altitude flight after PID operation. Fixed-point and tracking module uses the Openmv to sample the position of the black spot. After data processing, the control aircraft can keep above the target black spot to complete the fixed-point and tracking tasks. Under the control of MCU, the magnet module can complete the task of picking up and throwing iron sheets.

STM32 microcontroller; Flight attitude; Autonomous hovering; PID algorithm

TP

J

10.3969/j.issn.1003-6970.2018.11.026

鞏夢(mèng)巖，男，本科生，主要研究方向：物聯(lián)網(wǎng)工程；孟青，女，博士研究生，主要研究方向：電路與系統(tǒng)；程思強(qiáng)，男，本科生，主要研究方向：物聯(lián)網(wǎng)工程；李夢(mèng)，女，本科生，主要研究方向：物聯(lián)網(wǎng)工程。

鞏夢(mèng)巖，孟青，程思強(qiáng)，等. 基于STM32的四旋翼自主投靶飛行器[J]. 軟件，2018，39（11）：116-119