基于STM32 的室內四旋翼飛行器的設計與實現

張錦瑞，趙華磊，李試森，吳星華，馬益清

（陽光學院人工智能學院，福州350015）

0 引言

四旋翼飛行器具有可垂直升降、成本低、結構簡單等特點。其可應用于農藥噴灑、環境探測、高空拍攝、探索未知環境等功能。因此四旋翼飛行器的研究是近年來的熱點。當前有不少的團隊對四旋翼飛行器進行了相關的研究。其中王偉團隊設計基于微小飛行器姿態控制系統成本高，控制性能不穩定的特點，提出了采用廉價的角速度率陀螺儀和加速度計并結合姿態角模型，利用卡爾曼濾波器推測姿態角信息。來解決微小飛行器控制系統成本搞得問題。但該方案一般只能作為新手的試煉使用。因為其采集精度和頻率夠不高，會產生較大的溫漂和零漂。且其使用壽命周期短。王皓毅的團隊設計基于STM32F103C8T6 為核心采用MPU6050 傳感器設計飛行控制板，完成四旋翼飛行器控制軟硬的設計與開發，實現四旋翼左右、上下、前后的飛行要求。但該方案采用的MPU6050 傳感器相比于ICM20602 傳感器使得四旋翼飛行器噪音較大，無法適用于一些場合。 且STM32F103C8T6 相比于STM32F103RCT6 性能較差。針對以上情況，團隊提出應用STM32F103RCT6 微控制器，采用ICM20602 傳感器配合外設設施實現四旋翼飛行器的飛行和后期各種外設功能擴展，它能實現成品無人機，無法實現的擴展功能，并相比于大疆、小米等品牌四旋翼飛行器，成品機沒有開源，無法二次開發。應用STM32F103RCT6 微控制器的四旋翼飛行器，擁有更低的價格并且可以進行二次開發。

1 系統總體設計

整個四軸飛行器系統由七大模塊組成，分別為STM32F103RCT6 控制器、電源模塊、位置傳感器模塊（光流、OpenMV）、超聲波模塊、ICM20602 模塊、遙控接收模塊和電機。四翼飛行器主要通過ICM20602 和磁力計獲取四軸飛行器的此時的傾仰角、翻滾角、偏航角等參數，融合位置傳感器和超聲波的數據來輔助ICM20602 進行位置估計，最后通過實時獲取OpenMV對圖像進行二值化線性回歸計算黑線的角度和位置來進行巡線任務等。如圖1 所示。

圖1 系統總體框圖

2 系統硬件設計

2.1 微控制器的設計

該四軸飛行器選擇STM32 作為核心控制模塊。STM32F103RCT6 是以ARM Cortex-M3 處理器為內核設計的。它提供出色的計算性能和卓越的系統中斷響應能力。如圖2 所示。

圖2 STM32F103RCT6電路配置圖

STM32 控制器包含八個完全可編程的16C550 型UART。UART 能夠根據RX、TX、調制解調器流量控制、和錯誤條件產生獨立可屏蔽的中斷。如果任何中斷發生并且未被屏蔽，那么模塊將生成一個組合的中斷。并且通過I2C 總線獲取ICM20602 和IST8310 中的九軸數據再通過內部算法進行姿態解算。

2.2 電源模塊設計

該四軸飛行器選用AMS1117 降壓芯片來給MCU和板載傳感器供電，AMS1117 降壓芯片最大輸出電流為1A，完全足夠供電，并且在電源輸入端我們還加裝了兩個分壓電阻來對電池電壓進行采集，以保證無人機工作在正常的電量下。電路分別如圖3 所示。

圖3 電源電路圖

2.3 無刷電機模塊設計

該四軸飛行器選擇無刷電機來為其提供飛行動力。其無刷電機的型號是KV1400 朗宇A2212。如圖4 所示。

圖4 電機實物圖

電機作為四軸飛行器整個驅動系統中最重要的一環，要能為飛行器提供強大的飛行動力，確保四軸飛行器飛行的穩定性。一般無刷電機需要搭配電調使用，因無刷電機的轉速主要是通過電調來控制的，所以四軸飛行器選用20A 的電調，最大可以提供20A 的電流來控制電機的轉速。該電調原理如圖5 所示。

圖5 電調原理圖

2.4 光流傳感器模塊

光流就是利用飛行器底部的攝像頭通過采集圖像數據檢測地面的狀態，監測飛機的移動，然后通過PID控制算法實現飛行器穩定的定點懸停。這種定位手段配合GPS 可以在室外實現對飛行器的精準控制，并且在室內沒有GPS 信號的時候，也能對飛行器實現高精度的定位，實現更加穩定的控制。光流名字很專業，但道理很簡單，光流就是在這個世界中明顯感覺到的視覺運動。例如，坐在動車上，看外面發現樹木、物體在往后運動，這就是光流。

2.5 OpenMV模塊

在視覺模塊上，團隊采用OpenMV4 作為主要視覺傳感器。OpenMV4 可以滿足人臉識別、顏色追蹤、錄制視頻等額外擴展等功能。OpenMV 是一個開源、低成本、功能強大的機器視覺模塊。以STM32F427MCU為核心，集成了OV7725 攝像頭芯片，在小巧的硬件模塊上，用C 語言高效地實現了核心機器視覺算法，提供Python 編程接口可以用Python 語言使用OpenMV 提供的機器視覺功能，使用其內部集成的算法，對所示圖像進行圖像濾波、二值化、邊緣檢測等，通過不同的方式運用這些算法，可以提取出需要的圖像細節，然后上傳飛控交由飛控決策，例如說通過對圖像的二值化提取出圖像黑線，然后對黑線進行線性回歸，就能實現無人機的巡線。

3 系統軟件設計

3.1 總體程序設計

軟件設計中主要通過姿態計算和PID 控制算法，以實現對整個系統的穩定性。系統再接收遙控信號解出姿態的同時讀取自身攜帶姿態傳感器的數據并計算出飛行姿態。進行數據融合后。PID 控制器利用差值來計算出合適的電機輸出量，最終實現四軸飛行器的穩定飛行，再配合OpenMV 可以實現循跡功能。整個控制系統的流程圖如圖6 所示。

3.2 姿態解算

姿態其實就是指飛行器的翻滾、傾仰以及航向情況。姿態解算就是指利用自身攜帶的傳感器來檢測飛行器飛行數據，如線性加速度、角速度、磁場強度等，然后計算得出飛行器的飛行姿態的信息，利用PID 控制器計算出多個電機的輸出，使四軸飛行器飛行穩定。

3.3 坐標系轉換

因為飛行器在飛行過程中它的姿態是隨意變化的，所以必須建立坐標系來對姿態進行解算。坐標系可以分為N 系參考坐標和B 系載體坐標。

圖6 控制系統流程圖

N 系坐標和B 系坐標之間任何角度的關系都可看成有限的基本旋轉組合，繞X 軸旋轉，繞Y 軸旋轉，繞Z 軸旋轉，得出的基本旋轉矩陣分別為：

由旋轉矩陣求歐拉角。設三個軸x,y,z 的歐拉角分別為θx,θy,θz，正弦值、余弦值分別為sx,cx,sy,cy,sz,cz 那么旋轉矩陣為：

解方程可得：

（1）四元數求姿態角

四元數應用在姿態解算中，配合PID 算法讓飛行穩定，單位向量（x,y,z）旋轉θ角度后的四元數：其中R代表四元素旋轉矩陣，θ表示俯仰角。

可以轉化成矩陣的形式：

通過式4 和式7 比較，可以得出姿態角：

（2）PID 控制器

PID 控制器結構圖如圖7 所示。

圖7 PID控制器結構圖

括號內第一項是比例項，第二項是積分項，第三項是微分項。其輸入e（t）與輸出u（t）的關系為：u（t）=kp（e（t）+1/TI∫e（t）dt+TD*de（t）/dt）式中積分的上下限分別是0 和t。因此傳遞函數為：G（s）=U（s）/E（s）=kp（1+1/（TI*s）+TD*s），其中kp 為比例系數；TI 為積分時間常數；TD 為微分時間常數。

（3）梯度下降算法

梯度下降法其實是一種迭代求極值的方法在數據融合時，融合姿態角加速度等數據得到最佳姿態角。公式的實現流程圖如圖8 所示。

圖8

3.4 算法實現

飛行器通過PID 控制器計算出電機輸出量，最終是以PWM 占空比形式輸出進而驅動電機。

y（k）=y（k-1）+△y（增量），△y 是pwm 輸出變化量，y（k-1）是上一次輸出量，y（k）這次輸出量。具體過程如圖9 所示。

圖9 電機動力分配圖

在電機輸出計算中，需要用到油門信息量和偏航信息量。油門信息主要通過對遙控命令來實現。其中橫滾量參與左右電機輸出量的計算，俯仰量參與前后電機輸出量的計算。偏航信息、橫滾量以及俯仰量通過PID 計算獲取。利用主控芯片驅動OpenMV 從而實現對物體的識別與循跡。

4 實驗結果分析

4.1 軟硬件聯調

在完成飛行器的硬件設計和軟件設計之后，進行了飛行器的拼裝和飛行測試。主要測試了其飛行器的基本功能并且初步確立了飛行器的各項控制參數。最佳的控制參數需要進一步的研究，需要進行反復的實驗與調試，還需對飛行器姿態融合算法與PID 控制參數進行更細微的調整。

該四軸飛行器在如下條件和環境中進行了測試：

（1）采用4200mah 25c 3s 鋰電池對系統進行供電；

（2）在室內進行簡單的飛行運動，完成升降、定高懸停等基本動作；

（3）配合OpenMV 完成對物體的識別以及尋黑線的功能；飛行器實物如圖10 所示。

圖10 飛行器飛行圖

通過初步試飛實驗表明，該飛行器能夠完成基本的飛行動作，但是飛行會晃動，通過調節PID 來提高穩定性，提高P 和I 的值，飛行器趨于穩定。重新整定PID 的參數，飛行器基本的飛行動作都能很好地完成，飛行器能很好地平穩飛行配合OpenMV 四旋翼飛行器實現了物體識別、尋黑線等功能。

4.2 數據融合分析

數據融合主要包括了傳感器的數據處理和梯度下降算法。因為信號傳輸時和信號測量時都會產生巨大的干擾，同時也存在著外部磁干擾，所以需要對傳感器的數據進行處理。這樣也可以消除各芯片的靜態誤差。梯度下降算法的作用主要是為了三軸陀螺儀解算姿態角，利用磁力計和加速度計來補償陀螺儀積分的漂移誤差量。因為飛行器攜帶的傳感器都為三軸，所以選取其中單個軸向進行分析。在飛行器傳感器中，加速度計的初始數據非常最要。數據處理前后的對例如圖11 所示。

圖11 加速度計數據圖

由圖11 可知，要消除靜態誤差，必須對加速度計進行數據處理。以減少數據的高頻噪聲同時實現其數據的低通濾波功能。

ICM20602 姿態傳感器中加速度計可以對橫滾角和俯仰角進行修正，而磁力計可以對偏航角進行修正。因為數據融合主要圍繞消除陀螺儀數據的積分誤差展開，所以為了減少信號在傳輸過程中的干擾提高姿態數據的準確性，需對陀螺儀進行數據處理來消除積分誤差。

雖然陀螺儀數據可以直接進行姿態角解算，但是如果只用陀螺儀進行姿態解算，姿態角的漂移誤差會很大，得不到準確的姿態角，進行數據融合后，可以有效減少姿態角的漂移誤差，其效果非常明顯。如圖12為數據融合前后的波形對比。

圖12 加速度計修正圖

由圖12 可知，融合后的數據減小了漂移傾向，波形更加平穩。不僅降低了姿態角的漂移誤差，也增強了系統的抗干擾能力。

偏航角的修正主要通過磁力計來實現，如圖13所示。

圖13 磁力計計修正圖

由圖13 可知，融合前的偏航數據存在明顯的角度漂移且噪聲較大。經梯度下降算法融合后都得到了很好的控制。如圖14 為融合后的完整姿態數據波形。

4.3 總結

通過初步試飛實驗，進行融合數據分析和調節PID 算法，能進一步提高四旋翼飛行器的飛行穩定性，應用STM32F103RCT6 控制器，配合外設設備可以實現四旋翼飛行器的實地飛行，后期在配合外設OpenMV機器視覺模塊可以進行物體識別、巡線等功能。本作品參加2019 年全國大學生電子設計競賽，最終獲得全國二等獎。

圖14 姿態角數據波形圖