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

鞏夢巖，孟 青，程思強，李 夢

?

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

鞏夢巖，孟 青，程思強，李 夢

（中北大學，山西 朔州 036000）

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

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

0 引言

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

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

1.1 飛行器的模型建立

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

圖1 機體坐標圖

1.2 濾波器的設計

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

1.3 姿態(tài)解算

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

1.4 慣性導航

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

1.5 PID控制

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

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

2. 當前量（實際值）為rout(t)；

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

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

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

1.6 飛行高度控制

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

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

1.7 飛行路徑控制

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

圖2 超聲波原理圖

1.8 薄鐵片拾取與投放控制

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

2 電路與程序設計

2.1 系統(tǒng)整體框架設計

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

圖3 整體流程圖

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

圖4 整體電路圖

2.3 程序設計

程序流程圖如下所示：

圖5 程序流程圖

主要軟件設計如圖6所示。

圖6 軟件設計圖

3 測試方案與測試結果

3.1 測試方案

（1）硬件測試

檢查飛行器各部分的連接，卸掉飛行器的機槳，運行程序，檢查電調聲音，感受各個電機的轉速，測試期間將各部分數據發(fā)送至地面站，通過觀察數據和電路連接檢查電路各硬件部分狀況。

（2）軟件仿真測試

使用ST-Link線上調試飛控板，RL78G13仿真板線上調試主控制板，觀察軟件運行是否流暢。

（3）硬件軟件聯調

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

3.2 測試條件與儀器

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

測試儀器：學生電源，示波器，數字式萬用表。

3.3 測試結果及分析

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

表1 測試結果表

Tab.1 Test results table

4 結束語

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

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

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

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

[4] 靳亞磊, 李虹, 李昕濤. 四旋翼飛行器串級PID控制設計與實現[J]. 太原科技大學學報, 2018, 39(3): 177-183.

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

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

[7] 王忠生, 閆超. 四旋翼飛行器的懸停控制問題探討[J]. 赤峰學院學報(自然科學版), 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

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

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