基于STM32四旋翼飛行器設計與算法

楊洪亞　裴文卉

摘 要：近些年微機電系統（MEMS）的快速發展，現代控制的理論算法在四旋翼飛行器控制得到廣范運用。文章從四個部分對四旋翼飛行器的硬件設計與控制算法進行研究，首先分析飛行器原理與結構，建立動力學方程模型，然后對主控電路STM32F411CE和發射、接收電路NRF51822+RFX2401C的設計進行介紹，之后利用卡爾曼濾波算法將獲取MPU9250（加速度計和陀螺儀）的姿態數據進行融合，設計出一款超高性能PID控制器，最后利用數學工具MATLAB/Simulink對濾波前后的信號波形進行比較，突顯卡爾曼濾波算法先進性。

關鍵詞：動力學方程；NRF51822+RFX2401C；卡爾曼濾波算法； MATLAB/Simulink

中圖分類號：V249.1 文獻標志碼：A 文章編號：2095-2945（2018）17-0034-05

Abstract： In recent years， with the rapid development of Micro Electromechanical System （MEMS）， modern control theory and algorithm have been widely used in the control of four-rotor aircraft. In this paper， the hardware design and control algorithm of the four-rotor aircraft is studied in four parts. Firstly， the principle and structure of the vehicle are analyzed， and the dynamic equation model is established. Then the design of main control circuit STM32F411CE and transmitting and receiving circuit NRF51822 + RFX2401C is introduced. Then the attitude data obtained from MPU9250 （accelerometer and gyroscope） are fused by Kalman filter algorithm， and a super-high performance PID controller is designed. Finally， MATLAB/Simulink is used to compare the signal waveforms before and after filtering， which highlights the advancement of Kalman filtering algorithm.

Keywords： kinetic equation； NRF51822+RFX2401C； Kalman filter algorithm； MATLAB/Simulink

四旋翼飛行器因具備體積小，質量輕，結構簡單，可懸停和垂直起降等優點，可用于國防軍事，海洋執法監測和民用生活航拍領域。本文從工程設計角度建立卡爾曼狀態方程和測量方程，將姿態角度進行精密解算來對電機精確控制，實現空中翻轉、懸停等。

1 四旋翼飛行器的結構與原理

四旋翼飛行器模型采用十字交叉式，由四個直流電機驅動風葉組成的六自由度，欠驅動系統。因存在旋轉力矩與平衡系統的耦合問題，傳統的直升機通過借助尾槳調節，來平衡反扭矩， 因此四旋翼飛行器在設計時需要考慮多變量，非線性系統的問題。工作原理如下：

1.1 垂直上升、下降運動：飛行器自穩后，電機1，2，3，4號保持同樣速度，同時加速或減速，飛行器會發生垂直上升或下降運動。如圖1所示。

1.2 俯仰、橫滾（前后、左右）運動：飛行器自穩后，1號電機增速（減速），3號電機減速（加速），同時保持其他兩個電機轉速不變，這樣機體會存在拉力差，使機身前后傾斜，拉力產生水平分量，使機體向前運動。如圖2所示，同理，同樣的方法作用到2號、4號電機會發發生橫滾（左右）運動，如圖3所示

1.3 偏航運動：增大（減小）1號、3號電機，減小（增大）2號、4號電機轉速，機體會發生順時針偏航運動，反之為逆時針偏航運動。如圖4所示。

2 運動方程模型

為獲得四旋翼飛行器數學模型，我們可以假定a.四旋翼飛行器模型是均勻對稱的剛體；b.飛行器各個方向的拉力與推進器轉速的平方成正比。設立兩個基坐標參考系：慣性坐標系E（OXYZ）和飛行器坐標系B（OXYZ），如圖5所示。首先設定歐拉角？啄=[？準 ？茲 ？鬃]T，將飛行器經過三次簡單轉動得到模型的三個姿態角依次為？鬃→？茲→？準。這三次旋轉分別得到從機體坐標系到地面坐標系XYZ軸的轉換矩陣如下：

3 硬件電路設計

3.1 飛行器硬件系統主要組成

系統電路主要包括氣壓計定高模塊MS5611-01BA，無線收發模塊，陀螺加速度計模塊MPU9250，核心MCU STM32F411電路，空心杯直流電機等幾個重要模塊組成。

3.2 主控電路最小系統組成

本設計中采用ST公司的STM32F411RE最為主控芯片，該芯片是Cortex-M4內核，帶FPU（Float Point Unit，源點運算單元）和DSP（Digital Signal Processing，數字信號處理）指令集，主頻可達到168MHz（可獲得210DMIPS的處理能力，并且擁有128KB的SRAM（Static Random Access Memory，靜態隨機存取存儲器，帶攝像頭DCMI接口。還擁有ART（Adaptive Real-Time Accelerator，自適應實施加速器），可以達到相當于Flash零等待周期的性能。電路如圖6所示。

芯片采用雙晶振設計，8M晶振經過鎖相環可到180MHz，RTC實時時鐘采用32.768MHz晶振。

3.3 通訊電路設計

本文飛行器通訊電路采用NORDIC公司的低功耗藍牙芯片Nrf51822，該款芯片是32位ARM Cortex-M0，有256KB FLASH存儲，16KB RAM存儲，睡眠模式功耗電流僅為 0.4uA，正常工作電壓范圍為1.8-3.6V，最高輸出功率為4dbm，靈敏度為92.5dBm與RFX2401C結合可將發射距離擴大到100m。如圖7所示。

3.4 PCB設計

PCB設計采用四層板設計，材料選用FR-4，在RF 射頻電路設計時采用50歐阻抗匹配，通過PCB參數計算工具Saturn PCB Design Toolkit 如圖8所示。

差分電路阻抗匹配為100Ω，PCB層間距如圖9所示。

用Altium Designer軟件設計出的PCB如圖10所示。

4 算法部分-卡爾曼濾波器的PID控制

擴展卡爾曼濾波算法是一種高效的遞推算法，它被廣泛應用在工業和科研領域中，特別是飛行器控制中，具有很高的姿態解算精度。建立狀態方程，通過對系統狀態進行估算得到下一時刻的姿態角。卡爾曼濾波器的設計要求系統模型為離散模型。定義的線性狀態方程狀態空間模型為：

X（k）=AX（k-1）+BU（k）+W（k）

式中：X（k）為當前系統狀態的估算值，A為前一狀態X（k）和后一狀態X（k+1）的狀態轉移矩陣，U（k）為k時刻控制輸入；B為輸入控制，W（k）為k時刻系統的過程噪聲及系統誤差，當W（k）無限逼近于0，意味著過程噪聲W（k）為高斯白噪聲N（0，Qk），Qk為過程噪聲的協方差矩陣，反之，W（k）為有色噪聲。

觀測方程即系統輸出方程為 z（k）=HX（k）+V（k）

式中：H為測量相關狀態矩陣；z（k）為k時刻的觀測值，當加速度計的觀測噪聲V（k）逼近于0，意味著高斯白噪聲N（0，Rk），Rk為觀測噪聲協方差矩陣，反之，V（k）為有色噪聲。

基于卡爾曼濾波的PID控制結構圖如圖11所示。

建立狀態空間模型為：

xk=f（xk-1'+uk+wk）Zk=h（xk，Vk）

其中W（k）和V（k）均為隨機信號服從正態分布：

p（w）～N（0，Qk）p（v）～N（0，Rk）

協方差系數Rk與Qk是影響濾波效果和響應速度的重要參數，Rk取值越小，濾波響應和收斂越快；Qk取值越小，抑制、濾除噪聲的能力就越強。

卡爾曼濾波的基本公式為：

（1）狀態預測方程：由系統狀態變量k-1時刻的最優值X（k-1|k-1）和系統輸入U（k）可以求出k時刻系統預測值X（k|k-1）為

X（k|k-1）=AX（k-1|k-1）+BU（k）

（2）協方差預測方程：根據k-1時刻系統協方差矩陣 預測k時刻系統協方差矩陣P（k|k-1）為

P（k|k-1）=AP（k-1|k-1）AT+Q

（3）卡爾曼增益計算方程：根據狀態變量預測值和協方差矩陣預測值計算卡爾曼增益Kg（k）為

Kg（k）=P（k|k-1）HT/（HP（k|k-1）HT+R）

（4）最優值更新方程：由狀態變量預測值和系統測量值計算k時刻狀態變量最優值X（k|k）為

X（k|k）=X（k|k-1）+Kg（k）（Z（k）-HX（k|k-1））

（5）協方差更新方程：更新k時刻協方差矩陣P（k|k）為

P（k|k）=（1-Kg（k）H）P（k|k-1）

最優值和協方差更新方程都計算結束后，開始進行下一輪循環，往復（1）-（5）步。遞推算法的本質體現在估計過程，每次只需根據以前的測量值遞歸計算，就能得到當前時刻的狀態估計。卡爾曼濾波器的工作流程如圖12所示。

用MATLAB/Simulink仿真結果，如圖13所示。

5 結束語

本文通過對四旋翼飛行器硬件電路設計以及姿態傳感器獲取姿態角的數據解算，建立飛行器的數學模型，利用估計后的姿態角設計PID控制器實現對飛行器角度擾動的糾正。利用MATLAB仿真和分析，驗證卡爾曼濾波算法在飛行器控制應用中可以極大減小誤差。

參考文獻：

[1]唐建杰，王鑫.基于PI-PD控制器的四旋翼姿態控制[J].電子技術與軟件工程，2014.

[2]張棟，焦嵩鳴，劉延泉.互補濾波和卡爾曼濾波的融合姿態解算方法[J].傳感器與微系統2017，36（3）：62-66.

[3]馮新宇，范紅剛，辛亮.四旋翼無人飛行器設計[M].北京：清華大學出版社，2017：20.

[4]張帥.四旋翼飛行器穩定性的非線性控制[D].上海：上海電力學院，2017.

[5]王偉.四旋翼飛行器姿態控制系統設計[J].科學技術與工程，2013，13（19）：5514-5519.

[6]歷小偉，郭玉英.四旋翼飛行器的動力學建模與飛行控制[J].自動化與儀器儀表，2017，01：130-132.

[7]楊廣杰，白寶明，李凱.四旋翼無人飛行器飛行控制系統設計[D].西安：西安電子科技大學，2013.

[8]張麗娟，孫戈.四旋翼飛行器的姿態角估計與控制[D].西安：西安科技大學，2016.

[9]胡琦逸，王建中，鄒洪波.四旋翼飛行器的姿態估計與優化控制研究[D].杭州：杭州電子科技大學，2013.